From a3e04714b67ba8c633b288730d4a70b58f225686 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 13 Apr 2026 12:48:37 +0200 Subject: [PATCH 01/29] changes so build works --- atos/modules/EsminiAdapter/src/esminiadapter.cpp | 3 +-- scripts/installation/install_deps.sh | 2 +- scripts/installation/install_functions.sh | 10 +++++----- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/atos/modules/EsminiAdapter/src/esminiadapter.cpp b/atos/modules/EsminiAdapter/src/esminiadapter.cpp index 6b4da4917..50851c1ca 100644 --- a/atos/modules/EsminiAdapter/src/esminiadapter.cpp +++ b/atos/modules/EsminiAdapter/src/esminiadapter.cpp @@ -230,8 +230,7 @@ void EsminiAdapter::reportObjectPosition(const Monitor::message_type::SharedPtr auto speed = monr->velocity.twist.linear; // Reporting to Esmini - int timestamp = 0; // 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); } diff --git a/scripts/installation/install_deps.sh b/scripts/installation/install_deps.sh index 10385c45f..cc3647bfc 100755 --- a/scripts/installation/install_deps.sh +++ b/scripts/installation/install_deps.sh @@ -105,7 +105,7 @@ if [ -d "/usr/local/include/esmini" ] && [ "$REINSTALL" = false ]; then echo "esmini already installed, skipping installation..." else echo "Downloading esmini binaries..." - wget https://github.com/esmini/esmini/releases/download/v2.37.13/esmini-bin_Linux.zip -O $SOURCE_PATH/esmini-bin_Linux.zip + wget https://github.com/esmini/esmini/releases/download/v3.0.1/esmini-bin_Linux.zip -O $SOURCE_PATH/esmini-bin_Linux.zip check_command_failed $? "Failed to get esmini." unzip $SOURCE_PATH/esmini-bin_Linux.zip -d $SOURCE_PATH/esmini-bin_Linux cd $SOURCE_PATH/esmini-bin_Linux 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 239d04f59b8b8cabd6b34c7799e4ca05a8d15804 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 13 Apr 2026 13:57:52 +0200 Subject: [PATCH 02/29] ATOSMini scaffolding --- ATOSMini_architecture_and_dev_guide.pdf | Bin 0 -> 4248 bytes README.md | 21 +++ atos/CMakeLists.txt | 4 + atos/launch/launch_atosmini.py | 113 +++++++++++++ .../modules/TruckObjectControl/CMakeLists.txt | 38 +++++ .../inc/truckobjectcontrol.hpp | 50 ++++++ atos/modules/TruckObjectControl/src/main.cpp | 14 ++ .../src/truckobjectcontrol.cpp | 156 ++++++++++++++++++ atos_gui/atos_gui/main.py | 108 ++++++++---- atos_gui/atos_gui/objectpanel/objectpanel.py | 19 ++- atos_gui/setup.py | 3 +- 11 files changed, 484 insertions(+), 42 deletions(-) create mode 100644 ATOSMini_architecture_and_dev_guide.pdf create mode 100644 atos/launch/launch_atosmini.py create mode 100644 atos/modules/TruckObjectControl/CMakeLists.txt create mode 100644 atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp create mode 100644 atos/modules/TruckObjectControl/src/main.cpp create mode 100644 atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp 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/CMakeLists.txt b/atos/CMakeLists.txt index 2d83dba86..d8c4f0fe0 100644 --- a/atos/CMakeLists.txt +++ b/atos/CMakeLists.txt @@ -66,6 +66,7 @@ set(WITH_BACK_TO_START ON CACHE BOOL "Enable BackToStart module") set(WITH_OPEN_SCENARIO_GATEWAY ON CACHE BOOL "Enable OpenScenario Gateway module") set(WITH_REST_BRIDGE ON CACHE BOOL "Enable RESTBridge module") set(WITH_MONR_RELAY ON CACHE BOOL "Enable MonrRelay module") +set(WITH_TRUCK_OBJECT_CONTROL ON CACHE BOOL "Enable TruckObjectControl module") set(ENABLE_TESTS ON CACHE BOOL "Enable testing on build") @@ -106,6 +107,9 @@ endif() if(WITH_MONR_RELAY) list(APPEND ENABLED_MODULES MonrRelay) endif() +if(WITH_TRUCK_OBJECT_CONTROL) + list(APPEND ENABLED_MODULES TruckObjectControl) +endif() # Add corresponding subprojects 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 new file mode 100644 index 000000000..d5dceba83 --- /dev/null +++ b/atos/modules/TruckObjectControl/CMakeLists.txt @@ -0,0 +1,38 @@ +# This Source Code Form is subject to the terms of the Mozilla Public +# 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/. + +cmake_minimum_required(VERSION 3.8) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) + +project(truck_object_control) + +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +include(GNUInstallDirs) + +add_executable(${PROJECT_NAME} + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/truckobjectcontrol.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs +) + +install(CODE "MESSAGE(STATUS \"Installing target ${PROJECT_NAME}\")") +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp new file mode 100644 index 000000000..f1bf393e9 --- /dev/null +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -0,0 +1,50 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * 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/. + */ +#pragma once + +#include +#include + +#include +#include + +class TruckObjectControl : public rclcpp::Node { +public: + TruckObjectControl(); + +private: + struct TruckState { + double distance_along_trajectory_m = 0.0; + bool tcp_connected = false; + rclcpp::Time last_cot_stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); + }; + + struct CotObservation { + std::string truck_id; + double distance_along_trajectory_m = 0.0; + bool tcp_connected = false; + }; + + rclcpp::Subscription::SharedPtr cot_sub_; + rclcpp::Publisher::SharedPtr speed_command_pub_; + rclcpp::TimerBase::SharedPtr evaluation_timer_; + + std::unordered_map trucks_; + + double warning_distance_m_ = 400.0; + double stop_distance_m_ = 200.0; + double warning_speed_kmh_ = 30.0; + double stop_speed_kmh_ = 0.0; + double cot_timeout_seconds_ = 2.0; + + double last_published_speed_kmh_ = -1.0; + + void onCotMessage(const std_msgs::msg::String::SharedPtr msg); + void evaluateAndPublishSpeedCommand(); + + bool parseCotPlaceholder(const std::string &payload, CotObservation &out) const; + bool isCotFresh(const TruckState &state, const rclcpp::Time &now) const; +}; diff --git a/atos/modules/TruckObjectControl/src/main.cpp b/atos/modules/TruckObjectControl/src/main.cpp new file mode 100644 index 000000000..a8d56d02f --- /dev/null +++ b/atos/modules/TruckObjectControl/src/main.cpp @@ -0,0 +1,14 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * 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 "truckobjectcontrol.hpp" + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp new file mode 100644 index 000000000..cb0513c54 --- /dev/null +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -0,0 +1,156 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * 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 "truckobjectcontrol.hpp" + +#include +#include +#include +#include +#include + +using std::placeholders::_1; + +TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { + declare_parameter("warning_distance_m", warning_distance_m_); + declare_parameter("stop_distance_m", stop_distance_m_); + declare_parameter("warning_speed_kmh", warning_speed_kmh_); + declare_parameter("stop_speed_kmh", stop_speed_kmh_); + declare_parameter("cot_timeout_seconds", cot_timeout_seconds_); + + warning_distance_m_ = get_parameter("warning_distance_m").as_double(); + stop_distance_m_ = get_parameter("stop_distance_m").as_double(); + warning_speed_kmh_ = get_parameter("warning_speed_kmh").as_double(); + stop_speed_kmh_ = get_parameter("stop_speed_kmh").as_double(); + cot_timeout_seconds_ = get_parameter("cot_timeout_seconds").as_double(); + + cot_sub_ = create_subscription( + "truck_objects/cot", 50, std::bind(&TruckObjectControl::onCotMessage, this, _1)); + + speed_command_pub_ = create_publisher("truck_objects/speed_command", 20); + + evaluation_timer_ = create_wall_timer( + std::chrono::milliseconds(200), std::bind(&TruckObjectControl::evaluateAndPublishSpeedCommand, this)); + + RCLCPP_INFO(get_logger(), + "TruckObjectControl placeholder started. Waiting for COT on 'truck_objects/cot'."); + RCLCPP_INFO(get_logger(), + "Expected placeholder payload: id=;distance_m=;tcp_connected=<0|1>"); +} + +void TruckObjectControl::onCotMessage(const std_msgs::msg::String::SharedPtr msg) { + CotObservation observation; + if (!parseCotPlaceholder(msg->data, observation)) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "Failed to parse COT payload. Keeping placeholder behavior."); + return; + } + + auto &state = trucks_[observation.truck_id]; + state.distance_along_trajectory_m = observation.distance_along_trajectory_m; + state.tcp_connected = observation.tcp_connected; + state.last_cot_stamp = now(); +} + +bool TruckObjectControl::parseCotPlaceholder(const std::string &payload, CotObservation &out) const { + // Placeholder parser for early development only. + // Format: id=;distance_m=;tcp_connected=<0|1> + std::unordered_map fields; + std::stringstream ss(payload); + std::string token; + + while (std::getline(ss, token, ';')) { + const auto sep = token.find('='); + if (sep == std::string::npos) { + continue; + } + const std::string key = token.substr(0, sep); + const std::string value = token.substr(sep + 1); + fields[key] = value; + } + + if (fields.find("id") == fields.end() || fields.find("distance_m") == fields.end() || + fields.find("tcp_connected") == fields.end()) { + return false; + } + + out.truck_id = fields["id"]; + if (out.truck_id.empty()) { + return false; + } + + try { + out.distance_along_trajectory_m = std::stod(fields["distance_m"]); + const auto &tcp = fields["tcp_connected"]; + out.tcp_connected = (tcp == "1" || tcp == "true" || tcp == "TRUE"); + } catch (...) { + return false; + } + + return true; +} + +bool TruckObjectControl::isCotFresh(const TruckState &state, const rclcpp::Time &now_time) const { + const auto age = (now_time - state.last_cot_stamp).seconds(); + return age >= 0.0 && age <= cot_timeout_seconds_; +} + +void TruckObjectControl::evaluateAndPublishSpeedCommand() { + std::vector connected_positions; + std::vector connected_ids; + + const auto now_time = now(); + for (const auto &[id, state] : trucks_) { + if (!state.tcp_connected || !isCotFresh(state, now_time)) { + continue; + } + connected_positions.push_back(state.distance_along_trajectory_m); + connected_ids.push_back(id); + } + + if (connected_positions.size() < 2) { + return; + } + + std::sort(connected_positions.begin(), connected_positions.end()); + + double min_gap_m = std::numeric_limits::infinity(); + for (size_t i = 1; i < connected_positions.size(); ++i) { + min_gap_m = std::min(min_gap_m, std::fabs(connected_positions[i] - connected_positions[i - 1])); + } + + double target_speed_kmh = -1.0; + std::string reason = "no_limit"; + + if (min_gap_m < stop_distance_m_) { + target_speed_kmh = stop_speed_kmh_; + reason = "min_gap_below_stop_distance"; + } else if (min_gap_m < warning_distance_m_) { + target_speed_kmh = warning_speed_kmh_; + reason = "min_gap_below_warning_distance"; + } + + if (target_speed_kmh < 0.0) { + return; + } + + if (std::fabs(last_published_speed_kmh_ - target_speed_kmh) < 1e-6) { + return; + } + + std_msgs::msg::String command; + command.data = "target_speed_kmh=" + std::to_string(target_speed_kmh) + + ";scope=all_connected_with_valid_tcp_and_fresh_cot" + + ";reason=" + reason + + ";min_gap_m=" + std::to_string(min_gap_m) + + ";connected_count=" + std::to_string(connected_ids.size()); + + speed_command_pub_->publish(command); + last_published_speed_kmh_ = target_speed_kmh; + + RCLCPP_WARN(get_logger(), + "Published speed command %.1f km/h (reason=%s, min_gap=%.2f m, connected=%zu)", + target_speed_kmh, reason.c_str(), min_gap_m, connected_ids.size()); +} diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index e438f9707..593af7542 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -3,72 +3,110 @@ file, You can obtain one at https://mozilla.org/MPL/2.0/. """ +import sys import threading from pathlib import Path -from atos_interfaces.srv import * -import sys +import nicegui import rclpy +from nicegui import app, ui, ui_run from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor -from atos_gui.controlpanel.controlpanel import ControlPanelNode + from atos_gui.configpanel.configpanel import ConfigPanelNode +from atos_gui.controlpanel.controlpanel import ControlPanelNode from atos_gui.objectpanel.objectpanel import ObjectPanelNode -import nicegui -from nicegui import app, ui, ui_run +USE_SSL = len(sys.argv) > 1 and sys.argv[1] == "True" +ATOSMINI_MODE = len(sys.argv) > 2 and sys.argv[2].lower() == "atosmini" -USE_SSL = sys.argv[1] == "True" 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' + "uvicorn_reload_dirs": str(Path(__file__).parent.resolve()), + "port": 3000, + "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" + 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", flush=True) diff --git a/atos_gui/atos_gui/objectpanel/objectpanel.py b/atos_gui/atos_gui/objectpanel/objectpanel.py index e4548f8cd..319c5d510 100644 --- a/atos_gui/atos_gui/objectpanel/objectpanel.py +++ b/atos_gui/atos_gui/objectpanel/objectpanel.py @@ -7,6 +7,13 @@ MAX_TIMEOUT = 3 class ObjectPanelNode(Node): + 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: @@ -48,7 +55,7 @@ def get_object_ids(self): service_timeout_counter += 1 self.get_logger().debug('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._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) @@ -66,7 +73,7 @@ def get_object_ips(self, object_ids): service_timeout_counter += 1 self.get_logger().debug('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._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: @@ -92,12 +99,12 @@ 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._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)) @@ -112,12 +119,12 @@ 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._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._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): diff --git a/atos_gui/setup.py b/atos_gui/setup.py index 44ae0fab7..83fdf907e 100644 --- a/atos_gui/setup.py +++ b/atos_gui/setup.py @@ -21,7 +21,8 @@ zip_safe=True, entry_points={ 'console_scripts': [ - 'gui = atos_gui.main:main' + 'gui = atos_gui.main:main', + 'truck_object_gui = atos_gui.main:main' ], }, ) \ No newline at end of file From 7b1d20a9a2223f5112ecb9dc0d3199494fff77f4 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 13 Apr 2026 14:19:05 +0200 Subject: [PATCH 03/29] 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 593af7542..b60e21266 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" def main() -> None: @@ -26,15 +26,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` @@ -57,8 +57,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 74fd7cd1b1d12faffc874f9d833d0890c2003f07 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 13 Apr 2026 14:57:24 +0200 Subject: [PATCH 04/29] 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/29] simulation along path --- atos/launch/launch_atosfleetmanagement.py | 11 + .../modules/TruckObjectControl/CMakeLists.txt | 29 + .../inc/atostrucksimulator.hpp | 54 + .../inc/truckobjectcontrol.hpp | 40 + .../src/atostrucksimulator.cpp | 325 ++ .../TruckObjectControl/src/main_simulator.cpp | 14 + .../src/truckobjectcontrol.cpp | 441 +- atos_gui/atos_gui/main.py | 157 +- atos_gui/atos_gui/static/fleet_map.js | 240 + atos_gui/setup.py | 5 +- ...ralRoad_center_of_driving_lane_ccw.geojson | 4643 +++++++++++++++++ 11 files changed, 5926 insertions(+), 33 deletions(-) create mode 100644 atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp create mode 100644 atos/modules/TruckObjectControl/src/atostrucksimulator.cpp create mode 100644 atos/modules/TruckObjectControl/src/main_simulator.cpp 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/modules/TruckObjectControl/CMakeLists.txt b/atos/modules/TruckObjectControl/CMakeLists.txt index d5dceba83..aeef59d00 100644 --- a/atos/modules/TruckObjectControl/CMakeLists.txt +++ b/atos/modules/TruckObjectControl/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(std_msgs 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 @@ -22,6 +24,10 @@ add_executable(${PROJECT_NAME} target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../common +) +target_link_libraries(${PROJECT_NAME} + ${COREUTILS_LIBRARY} ) ament_target_dependencies(${PROJECT_NAME} @@ -29,6 +35,22 @@ ament_target_dependencies(${PROJECT_NAME} std_msgs ) +add_executable(atos_truck_simulator + ${CMAKE_CURRENT_SOURCE_DIR}/src/main_simulator.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/atostrucksimulator.cpp +) +target_include_directories(atos_truck_simulator PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../common +) +target_link_libraries(atos_truck_simulator + ${COREUTILS_LIBRARY} +) +ament_target_dependencies(atos_truck_simulator + rclcpp + std_msgs +) + install(CODE "MESSAGE(STATUS \"Installing target ${PROJECT_NAME}\")") install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" @@ -36,3 +58,10 @@ install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) +install(CODE "MESSAGE(STATUS \"Installing target atos_truck_simulator\")") +install(TARGETS atos_truck_simulator + RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) diff --git a/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp b/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp new file mode 100644 index 000000000..bcac53350 --- /dev/null +++ b/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp @@ -0,0 +1,54 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * 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/. + */ +#pragma once + +#include +#include + +#include +#include + +class AtosTruckSimulator : public rclcpp::Node { +public: + AtosTruckSimulator(); + +private: + struct GeoPoint { + double lat = 0.0; + double lon = 0.0; + double distance_m = 0.0; + }; + + std::string uid_ = "L5S-TRUCK-SIM"; + std::string tcp_host_ = "127.0.0.1"; + int tcp_port_ = 8114; + std::string trajectory_geojson_path_ = "/home/sepast/atos_ws/src/atos/conf/conf/RuralRoad_center_of_driving_lane_ccw.geojson"; + int start_index_ = 0; + double initial_speed_kmh_ = 0.0; + double target_speed_kmh_ = 40.0; + double acceleration_mps2_ = 0.8; + double publish_hz_ = 5.0; + bool loop_path_ = true; + + double current_distance_m_ = 0.0; + double current_speed_mps_ = 0.0; + + int tcp_fd_ = -1; + std::vector trajectory_path_; + + rclcpp::TimerBase::SharedPtr simulation_timer_; + rclcpp::Subscription::SharedPtr speed_command_sub_; + + rclcpp::Time last_step_time_; + + bool loadTrajectoryPath(); + bool ensureTcpConnected(); + void closeTcp(); + void simulationStep(); + bool pointAtDistance(double distance_m, double &lat, double &lon, double &course_deg) const; + static std::string utcIso8601FromRosTime(const rclcpp::Time &time); + void onSpeedCommand(const std_msgs::msg::String::SharedPtr msg); +}; diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index f1bf393e9..6531ba7d8 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -8,16 +8,31 @@ #include #include +#include +#include #include +#include #include +#include class TruckObjectControl : public rclcpp::Node { public: TruckObjectControl(); + ~TruckObjectControl() override; private: + struct GeoPoint { + double lat = 0.0; + double lon = 0.0; + double distance_m = 0.0; + }; + struct TruckState { double distance_along_trajectory_m = 0.0; + double lat = 0.0; + double lon = 0.0; + double speed_kmh = 0.0; + double course_deg = 0.0; bool tcp_connected = false; rclcpp::Time last_cot_stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); }; @@ -25,26 +40,51 @@ class TruckObjectControl : public rclcpp::Node { struct CotObservation { std::string truck_id; double distance_along_trajectory_m = 0.0; + double lat = 0.0; + double lon = 0.0; + double speed_kmh = 0.0; + double course_deg = 0.0; bool tcp_connected = false; }; rclcpp::Subscription::SharedPtr cot_sub_; rclcpp::Publisher::SharedPtr speed_command_pub_; + rclcpp::Publisher::SharedPtr truck_state_pub_; rclcpp::TimerBase::SharedPtr evaluation_timer_; + std::mutex state_mutex_; std::unordered_map trucks_; + std::vector trajectory_path_; double warning_distance_m_ = 400.0; double stop_distance_m_ = 200.0; double warning_speed_kmh_ = 30.0; double stop_speed_kmh_ = 0.0; double cot_timeout_seconds_ = 2.0; + int cot_tcp_port_ = 8114; + std::string cot_tcp_bind_address_ = "0.0.0.0"; + std::string trajectory_geojson_path_ = "/home/sepast/atos_ws/src/atos/conf/conf/RuralRoad_center_of_driving_lane_ccw.geojson"; double last_published_speed_kmh_ = -1.0; + std::atomic tcp_running_{false}; + int tcp_server_fd_ = -1; + std::thread tcp_accept_thread_; + std::vector tcp_client_threads_; + std::mutex tcp_threads_mutex_; + void onCotMessage(const std_msgs::msg::String::SharedPtr msg); void evaluateAndPublishSpeedCommand(); + void publishTruckState(const std::string &truck_id, const TruckState &state); bool parseCotPlaceholder(const std::string &payload, CotObservation &out) const; + bool parseCotXml(const std::string &payload, CotObservation &out) const; bool isCotFresh(const TruckState &state, const rclcpp::Time &now) const; + bool loadTrajectoryPath(); + double projectDistanceAlongTrajectory(double lat, double lon) const; + + void startTcpServer(); + void stopTcpServer(); + void acceptTcpClients(); + void handleTcpClient(int client_fd, const std::string &peer_name); }; diff --git a/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp new file mode 100644 index 000000000..d47a51a63 --- /dev/null +++ b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp @@ -0,0 +1,325 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * 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 "atostrucksimulator.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using json = nlohmann::json; + +namespace { +constexpr double kEpsilon = 1e-9; + +double degToRad(double value) { + return value * M_PI / 180.0; +} + +double radToDeg(double value) { + return value * 180.0 / M_PI; +} + +double geodesicDistanceMeters(double lat1, double lon1, double lat2, double lon2) { + const double earth_radius_m = 6378137.0; + const double dlat = degToRad(lat2 - lat1); + const double dlon = degToRad(lon2 - lon1); + const double a = std::sin(dlat / 2.0) * std::sin(dlat / 2.0) + + std::cos(degToRad(lat1)) * std::cos(degToRad(lat2)) * + std::sin(dlon / 2.0) * std::sin(dlon / 2.0); + const double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a)); + return earth_radius_m * c; +} +} + +AtosTruckSimulator::AtosTruckSimulator() : Node("atos_truck_simulator") { + declare_parameter("uid", uid_); + declare_parameter("tcp_host", tcp_host_); + declare_parameter("tcp_port", tcp_port_); + declare_parameter("trajectory_geojson_path", trajectory_geojson_path_); + declare_parameter("start_index", start_index_); + declare_parameter("initial_speed_kmh", initial_speed_kmh_); + declare_parameter("target_speed_kmh", target_speed_kmh_); + declare_parameter("acceleration_mps2", acceleration_mps2_); + declare_parameter("publish_hz", publish_hz_); + declare_parameter("loop_path", loop_path_); + + uid_ = get_parameter("uid").as_string(); + tcp_host_ = get_parameter("tcp_host").as_string(); + tcp_port_ = get_parameter("tcp_port").as_int(); + trajectory_geojson_path_ = get_parameter("trajectory_geojson_path").as_string(); + start_index_ = get_parameter("start_index").as_int(); + initial_speed_kmh_ = get_parameter("initial_speed_kmh").as_double(); + target_speed_kmh_ = get_parameter("target_speed_kmh").as_double(); + acceleration_mps2_ = std::max(0.01, get_parameter("acceleration_mps2").as_double()); + publish_hz_ = std::max(1.0, get_parameter("publish_hz").as_double()); + loop_path_ = get_parameter("loop_path").as_bool(); + + if (!loadTrajectoryPath()) { + RCLCPP_ERROR(get_logger(), "Failed to load trajectory at '%s'", trajectory_geojson_path_.c_str()); + return; + } + + if (start_index_ < 0) { + start_index_ = 0; + } + if (static_cast(start_index_) >= trajectory_path_.size()) { + start_index_ = static_cast(trajectory_path_.size() - 1); + } + + current_distance_m_ = trajectory_path_[static_cast(start_index_)].distance_m; + current_speed_mps_ = std::max(0.0, initial_speed_kmh_ / 3.6); + last_step_time_ = now(); + + speed_command_sub_ = create_subscription( + "truck_objects/speed_command", 20, + std::bind(&AtosTruckSimulator::onSpeedCommand, this, std::placeholders::_1)); + + const auto period_ms = static_cast(1000.0 / publish_hz_); + simulation_timer_ = create_wall_timer(std::chrono::milliseconds(period_ms), + std::bind(&AtosTruckSimulator::simulationStep, this)); + + RCLCPP_INFO(get_logger(), + "AtosTruckSimulator started (uid=%s, start_index=%d, target=%.1f km/h, accel=%.2f m/s^2, tcp=%s:%d)", + uid_.c_str(), start_index_, target_speed_kmh_, acceleration_mps2_, tcp_host_.c_str(), tcp_port_); +} + +bool AtosTruckSimulator::loadTrajectoryPath() { + trajectory_path_.clear(); + + std::ifstream input(trajectory_geojson_path_); + if (!input.is_open()) { + return false; + } + + json root; + try { + input >> root; + } catch (...) { + return false; + } + + if (!root.contains("features") || !root["features"].is_array()) { + return false; + } + + json line_feature; + for (const auto &feature : root["features"]) { + if (!feature.contains("geometry")) { + continue; + } + const auto &geometry = feature["geometry"]; + if (!geometry.contains("type") || geometry["type"] != "LineString") { + continue; + } + if (feature.contains("id") && feature["id"] == "PathSection") { + line_feature = feature; + break; + } + if (line_feature.is_null()) { + line_feature = feature; + } + } + + if (line_feature.is_null()) { + return false; + } + + const auto &coords = line_feature["geometry"]["coordinates"]; + if (!coords.is_array() || coords.empty()) { + return false; + } + + double cumulative = 0.0; + GeoPoint prev; + bool has_prev = false; + + for (const auto &coord : coords) { + if (!coord.is_array() || coord.size() < 2) { + continue; + } + GeoPoint p; + p.lon = coord[0].get(); + p.lat = coord[1].get(); + p.distance_m = cumulative; + + if (has_prev) { + cumulative += geodesicDistanceMeters(prev.lat, prev.lon, p.lat, p.lon); + p.distance_m = cumulative; + } + + trajectory_path_.push_back(p); + prev = p; + has_prev = true; + } + + return trajectory_path_.size() >= 2; +} + +bool AtosTruckSimulator::ensureTcpConnected() { + if (tcp_fd_ >= 0) { + return true; + } + + tcp_fd_ = ::socket(AF_INET, SOCK_STREAM, 0); + if (tcp_fd_ < 0) { + return false; + } + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(static_cast(tcp_port_)); + if (::inet_pton(AF_INET, tcp_host_.c_str(), &addr.sin_addr) != 1) { + closeTcp(); + return false; + } + + if (::connect(tcp_fd_, reinterpret_cast(&addr), sizeof(addr)) < 0) { + closeTcp(); + return false; + } + + return true; +} + +void AtosTruckSimulator::closeTcp() { + if (tcp_fd_ >= 0) { + ::shutdown(tcp_fd_, SHUT_RDWR); + ::close(tcp_fd_); + tcp_fd_ = -1; + } +} + +std::string AtosTruckSimulator::utcIso8601FromRosTime(const rclcpp::Time &time) { + const auto seconds = static_cast(time.seconds()); + std::tm tm_utc{}; + gmtime_r(&seconds, &tm_utc); + std::ostringstream out; + out << std::put_time(&tm_utc, "%Y-%m-%dT%H:%M:%SZ"); + return out.str(); +} + +bool AtosTruckSimulator::pointAtDistance(double distance_m, double &lat, double &lon, double &course_deg) const { + if (trajectory_path_.size() < 2) { + return false; + } + + const double max_distance = trajectory_path_.back().distance_m; + double d = distance_m; + if (loop_path_ && max_distance > kEpsilon) { + d = std::fmod(distance_m, max_distance); + if (d < 0.0) { + d += max_distance; + } + } else { + d = std::clamp(d, 0.0, max_distance); + } + + size_t segment_index = 1; + while (segment_index < trajectory_path_.size() && trajectory_path_[segment_index].distance_m < d) { + ++segment_index; + } + if (segment_index >= trajectory_path_.size()) { + segment_index = trajectory_path_.size() - 1; + } + if (segment_index == 0) { + segment_index = 1; + } + + const auto &a = trajectory_path_[segment_index - 1]; + const auto &b = trajectory_path_[segment_index]; + const double segment_length = std::max(kEpsilon, b.distance_m - a.distance_m); + const double t = std::clamp((d - a.distance_m) / segment_length, 0.0, 1.0); + + lat = a.lat + (b.lat - a.lat) * t; + lon = a.lon + (b.lon - a.lon) * t; + + const double y = std::sin(degToRad(b.lon - a.lon)) * std::cos(degToRad(b.lat)); + const double x = std::cos(degToRad(a.lat)) * std::sin(degToRad(b.lat)) - + std::sin(degToRad(a.lat)) * std::cos(degToRad(b.lat)) * + std::cos(degToRad(b.lon - a.lon)); + course_deg = std::fmod(radToDeg(std::atan2(y, x)) + 360.0, 360.0); + return true; +} + +void AtosTruckSimulator::simulationStep() { + const auto now_time = now(); + const double dt = std::max(0.001, (now_time - last_step_time_).seconds()); + last_step_time_ = now_time; + + const double target_speed_mps = std::max(0.0, target_speed_kmh_ / 3.6); + const double max_delta = acceleration_mps2_ * dt; + const double delta = std::clamp(target_speed_mps - current_speed_mps_, -max_delta, max_delta); + current_speed_mps_ += delta; + + current_distance_m_ += current_speed_mps_ * dt; + if (!loop_path_) { + current_distance_m_ = std::clamp(current_distance_m_, 0.0, trajectory_path_.back().distance_m); + } + + double lat = 0.0; + double lon = 0.0; + double course_deg = 0.0; + if (!pointAtDistance(current_distance_m_, lat, lon, course_deg)) { + return; + } + + if (!ensureTcpConnected()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "Unable to connect to TruckObjectControl TCP %s:%d", + tcp_host_.c_str(), tcp_port_); + return; + } + + const std::string time_str = utcIso8601FromRosTime(now_time); + const std::string stale_str = utcIso8601FromRosTime(now_time + rclcpp::Duration::from_seconds(10.0)); + + std::ostringstream cot; + cot << "<__group name=\"Dark Green\" role=\"Test Vehicle\"/>"; + + const std::string payload = cot.str(); + const ssize_t sent = ::send(tcp_fd_, payload.data(), payload.size(), MSG_NOSIGNAL); + if (sent < 0) { + RCLCPP_WARN(get_logger(), "TCP send failed, reconnecting..."); + closeTcp(); + return; + } + + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, + "Sim uid=%s distance=%.1f m speed=%.1f km/h target=%.1f km/h lat=%.7f lon=%.7f", + uid_.c_str(), current_distance_m_, current_speed_mps_ * 3.6, target_speed_kmh_, lat, lon); +} + +void AtosTruckSimulator::onSpeedCommand(const std_msgs::msg::String::SharedPtr msg) { + const auto key = std::string("target_speed_kmh="); + const auto pos = msg->data.find(key); + if (pos == std::string::npos) { + return; + } + const auto value_start = pos + key.size(); + const auto value_end = msg->data.find(';', value_start); + const auto value = msg->data.substr(value_start, value_end - value_start); + try { + target_speed_kmh_ = std::max(0.0, std::stod(value)); + } catch (...) { + return; + } +} diff --git a/atos/modules/TruckObjectControl/src/main_simulator.cpp b/atos/modules/TruckObjectControl/src/main_simulator.cpp new file mode 100644 index 000000000..c4b63e061 --- /dev/null +++ b/atos/modules/TruckObjectControl/src/main_simulator.cpp @@ -0,0 +1,14 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * 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 "atostrucksimulator.hpp" + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index cb0513c54..7be548f12 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -5,13 +5,44 @@ */ #include "truckobjectcontrol.hpp" +#include +#include +#include +#include + #include #include +#include +#include #include +#include +#include +#include #include #include using std::placeholders::_1; +using json = nlohmann::json; + +namespace { +constexpr size_t kReceiveBufferSize = 4096; +constexpr int kAcceptPollSleepMs = 100; + +double degToRad(double value) { + return value * M_PI / 180.0; +} + +double geodesicDistanceMeters(double lat1, double lon1, double lat2, double lon2) { + const double earth_radius_m = 6378137.0; + const double dlat = degToRad(lat2 - lat1); + const double dlon = degToRad(lon2 - lon1); + const double a = std::sin(dlat / 2.0) * std::sin(dlat / 2.0) + + std::cos(degToRad(lat1)) * std::cos(degToRad(lat2)) * + std::sin(dlon / 2.0) * std::sin(dlon / 2.0); + const double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a)); + return earth_radius_m * c; +} +} // namespace TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { declare_parameter("warning_distance_m", warning_distance_m_); @@ -19,44 +50,90 @@ TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { declare_parameter("warning_speed_kmh", warning_speed_kmh_); declare_parameter("stop_speed_kmh", stop_speed_kmh_); declare_parameter("cot_timeout_seconds", cot_timeout_seconds_); + declare_parameter("cot_tcp_port", cot_tcp_port_); + declare_parameter("cot_tcp_bind_address", cot_tcp_bind_address_); + declare_parameter("trajectory_geojson_path", trajectory_geojson_path_); warning_distance_m_ = get_parameter("warning_distance_m").as_double(); stop_distance_m_ = get_parameter("stop_distance_m").as_double(); warning_speed_kmh_ = get_parameter("warning_speed_kmh").as_double(); stop_speed_kmh_ = get_parameter("stop_speed_kmh").as_double(); cot_timeout_seconds_ = get_parameter("cot_timeout_seconds").as_double(); + cot_tcp_port_ = get_parameter("cot_tcp_port").as_int(); + cot_tcp_bind_address_ = get_parameter("cot_tcp_bind_address").as_string(); + trajectory_geojson_path_ = get_parameter("trajectory_geojson_path").as_string(); cot_sub_ = create_subscription( "truck_objects/cot", 50, std::bind(&TruckObjectControl::onCotMessage, this, _1)); speed_command_pub_ = create_publisher("truck_objects/speed_command", 20); + truck_state_pub_ = create_publisher("truck_objects/state", 50); evaluation_timer_ = create_wall_timer( std::chrono::milliseconds(200), std::bind(&TruckObjectControl::evaluateAndPublishSpeedCommand, this)); + if (!loadTrajectoryPath()) { + RCLCPP_WARN(get_logger(), + "Failed to load trajectory path from '%s'. Distance along trajectory will stay 0.", + trajectory_geojson_path_.c_str()); + } else { + RCLCPP_INFO(get_logger(), "Loaded trajectory path with %zu points from %s", trajectory_path_.size(), + trajectory_geojson_path_.c_str()); + } + + startTcpServer(); + RCLCPP_INFO(get_logger(), - "TruckObjectControl placeholder started. Waiting for COT on 'truck_objects/cot'."); - RCLCPP_INFO(get_logger(), - "Expected placeholder payload: id=;distance_m=;tcp_connected=<0|1>"); + "TruckObjectControl started. Listening for COT XML on tcp://%s:%d and placeholder topic 'truck_objects/cot'.", + cot_tcp_bind_address_.c_str(), cot_tcp_port_); +} + +TruckObjectControl::~TruckObjectControl() { + stopTcpServer(); +} + +void TruckObjectControl::publishTruckState(const std::string &truck_id, const TruckState &state) { + json payload; + payload["uid"] = truck_id; + payload["distance_m"] = state.distance_along_trajectory_m; + payload["lat"] = state.lat; + payload["lon"] = state.lon; + payload["speed_kmh"] = state.speed_kmh; + payload["course_deg"] = state.course_deg; + payload["tcp_connected"] = state.tcp_connected; + payload["stamp_sec"] = now().seconds(); + + std_msgs::msg::String msg; + msg.data = payload.dump(); + truck_state_pub_->publish(msg); } void TruckObjectControl::onCotMessage(const std_msgs::msg::String::SharedPtr msg) { CotObservation observation; - if (!parseCotPlaceholder(msg->data, observation)) { + if (!parseCotPlaceholder(msg->data, observation) && !parseCotXml(msg->data, observation)) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, - "Failed to parse COT payload. Keeping placeholder behavior."); + "Failed to parse COT payload from ROS topic."); return; } - auto &state = trucks_[observation.truck_id]; - state.distance_along_trajectory_m = observation.distance_along_trajectory_m; - state.tcp_connected = observation.tcp_connected; - state.last_cot_stamp = now(); + TruckState state; + { + std::lock_guard lock(state_mutex_); + auto &entry = trucks_[observation.truck_id]; + entry.distance_along_trajectory_m = observation.distance_along_trajectory_m; + entry.lat = observation.lat; + entry.lon = observation.lon; + entry.speed_kmh = observation.speed_kmh; + entry.course_deg = observation.course_deg; + entry.tcp_connected = observation.tcp_connected; + entry.last_cot_stamp = now(); + state = entry; + } + + publishTruckState(observation.truck_id, state); } bool TruckObjectControl::parseCotPlaceholder(const std::string &payload, CotObservation &out) const { - // Placeholder parser for early development only. - // Format: id=;distance_m=;tcp_connected=<0|1> std::unordered_map fields; std::stringstream ss(payload); std::string token; @@ -76,15 +153,26 @@ bool TruckObjectControl::parseCotPlaceholder(const std::string &payload, CotObse return false; } - out.truck_id = fields["id"]; + out.truck_id = fields.at("id"); if (out.truck_id.empty()) { return false; } try { - out.distance_along_trajectory_m = std::stod(fields["distance_m"]); - const auto &tcp = fields["tcp_connected"]; + out.distance_along_trajectory_m = std::stod(fields.at("distance_m")); + const auto &tcp = fields.at("tcp_connected"); out.tcp_connected = (tcp == "1" || tcp == "true" || tcp == "TRUE"); + + if (fields.find("lat") != fields.end() && fields.find("lon") != fields.end()) { + out.lat = std::stod(fields.at("lat")); + out.lon = std::stod(fields.at("lon")); + } + if (fields.find("speed_kmh") != fields.end()) { + out.speed_kmh = std::stod(fields.at("speed_kmh")); + } + if (fields.find("course_deg") != fields.end()) { + out.course_deg = std::stod(fields.at("course_deg")); + } } catch (...) { return false; } @@ -92,22 +180,332 @@ bool TruckObjectControl::parseCotPlaceholder(const std::string &payload, CotObse return true; } +bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation &out) const { + // Expected input resembles: + // + static const std::regex uid_re(R"re(uid="([^"]+)")re"); + static const std::regex point_re(R"re(]*\blat="([^"]+)"[^>]*\blon="([^"]+)")re"); + static const std::regex track_re(R"re(]*\bspeed="([^"]+)"[^>]*\bcourse="([^"]+)")re"); + + std::smatch m; + if (!std::regex_search(payload, m, uid_re) || m.size() < 2) { + return false; + } + out.truck_id = m[1].str(); + if (out.truck_id.empty()) { + return false; + } + + if (!std::regex_search(payload, m, point_re) || m.size() < 3) { + return false; + } + + try { + out.lat = std::stod(m[1].str()); + out.lon = std::stod(m[2].str()); + } catch (...) { + return false; + } + + out.speed_kmh = 0.0; + out.course_deg = 0.0; + if (std::regex_search(payload, m, track_re) && m.size() >= 3) { + try { + // The incoming example uses speed in m/s; convert to km/h for command logic and UI. + const double speed_ms = std::stod(m[1].str()); + out.speed_kmh = speed_ms * 3.6; + out.course_deg = std::stod(m[2].str()); + } catch (...) { + out.speed_kmh = 0.0; + out.course_deg = 0.0; + } + } + + out.distance_along_trajectory_m = projectDistanceAlongTrajectory(out.lat, out.lon); + out.tcp_connected = true; + return true; +} + bool TruckObjectControl::isCotFresh(const TruckState &state, const rclcpp::Time &now_time) const { const auto age = (now_time - state.last_cot_stamp).seconds(); return age >= 0.0 && age <= cot_timeout_seconds_; } +bool TruckObjectControl::loadTrajectoryPath() { + trajectory_path_.clear(); + + std::ifstream input(trajectory_geojson_path_); + if (!input.is_open()) { + return false; + } + + json root; + try { + input >> root; + } catch (...) { + return false; + } + + if (!root.contains("features") || !root["features"].is_array()) { + return false; + } + + json line_feature; + for (const auto &feature : root["features"]) { + if (!feature.contains("geometry")) { + continue; + } + const auto &geometry = feature["geometry"]; + if (!geometry.contains("type") || geometry["type"] != "LineString") { + continue; + } + if (feature.contains("id") && feature["id"] == "PathSection") { + line_feature = feature; + break; + } + if (line_feature.is_null()) { + line_feature = feature; + } + } + + if (line_feature.is_null()) { + return false; + } + + const auto &coords = line_feature["geometry"]["coordinates"]; + if (!coords.is_array() || coords.empty()) { + return false; + } + + double cumulative = 0.0; + GeoPoint prev; + bool has_prev = false; + + for (const auto &coord : coords) { + if (!coord.is_array() || coord.size() < 2) { + continue; + } + const double lon = coord[0].get(); + const double lat = coord[1].get(); + + GeoPoint p; + p.lat = lat; + p.lon = lon; + p.distance_m = cumulative; + + if (has_prev) { + cumulative += geodesicDistanceMeters(prev.lat, prev.lon, p.lat, p.lon); + p.distance_m = cumulative; + } + + trajectory_path_.push_back(p); + prev = p; + has_prev = true; + } + + return trajectory_path_.size() >= 2; +} + +double TruckObjectControl::projectDistanceAlongTrajectory(double lat, double lon) const { + if (trajectory_path_.empty()) { + return 0.0; + } + + double best_distance_to_path_m = std::numeric_limits::infinity(); + double best_distance_along_m = 0.0; + for (const auto &p : trajectory_path_) { + const double d = geodesicDistanceMeters(lat, lon, p.lat, p.lon); + if (d < best_distance_to_path_m) { + best_distance_to_path_m = d; + best_distance_along_m = p.distance_m; + } + } + return best_distance_along_m; +} + +void TruckObjectControl::startTcpServer() { + tcp_server_fd_ = ::socket(AF_INET, SOCK_STREAM, 0); + if (tcp_server_fd_ < 0) { + RCLCPP_ERROR(get_logger(), "Failed to create TCP socket for COT listener."); + return; + } + + int enable = 1; + (void)setsockopt(tcp_server_fd_, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(static_cast(cot_tcp_port_)); + if (::inet_pton(AF_INET, cot_tcp_bind_address_.c_str(), &addr.sin_addr) != 1) { + RCLCPP_ERROR(get_logger(), "Invalid bind address '%s' for COT TCP listener.", + cot_tcp_bind_address_.c_str()); + ::close(tcp_server_fd_); + tcp_server_fd_ = -1; + return; + } + + if (::bind(tcp_server_fd_, reinterpret_cast(&addr), sizeof(addr)) < 0) { + RCLCPP_ERROR(get_logger(), "Failed to bind COT TCP listener on %s:%d", cot_tcp_bind_address_.c_str(), + cot_tcp_port_); + ::close(tcp_server_fd_); + tcp_server_fd_ = -1; + return; + } + + if (::listen(tcp_server_fd_, 8) < 0) { + RCLCPP_ERROR(get_logger(), "Failed to listen on COT TCP listener."); + ::close(tcp_server_fd_); + tcp_server_fd_ = -1; + return; + } + + tcp_running_.store(true); + tcp_accept_thread_ = std::thread(&TruckObjectControl::acceptTcpClients, this); +} + +void TruckObjectControl::stopTcpServer() { + tcp_running_.store(false); + + if (tcp_server_fd_ >= 0) { + ::shutdown(tcp_server_fd_, SHUT_RDWR); + ::close(tcp_server_fd_); + tcp_server_fd_ = -1; + } + + if (tcp_accept_thread_.joinable()) { + tcp_accept_thread_.join(); + } + + std::lock_guard lock(tcp_threads_mutex_); + for (auto &thread : tcp_client_threads_) { + if (thread.joinable()) { + thread.join(); + } + } + tcp_client_threads_.clear(); +} + +void TruckObjectControl::acceptTcpClients() { + while (tcp_running_.load()) { + sockaddr_in client_addr{}; + socklen_t client_len = sizeof(client_addr); + const int client_fd = + ::accept(tcp_server_fd_, reinterpret_cast(&client_addr), &client_len); + if (client_fd < 0) { + if (tcp_running_.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(kAcceptPollSleepMs)); + } + continue; + } + + char ip_buf[INET_ADDRSTRLEN] = {0}; + const char *peer_ip = ::inet_ntop(AF_INET, &client_addr.sin_addr, ip_buf, sizeof(ip_buf)); + std::ostringstream peer; + peer << (peer_ip ? peer_ip : "unknown") << ":" << ntohs(client_addr.sin_port); + + RCLCPP_INFO(get_logger(), "Accepted TruckObject TCP client %s", peer.str().c_str()); + + std::lock_guard lock(tcp_threads_mutex_); + tcp_client_threads_.emplace_back(&TruckObjectControl::handleTcpClient, this, client_fd, peer.str()); + } +} + +void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_name) { + std::string buffer; + buffer.reserve(8 * 1024); + std::set seen_uids; + + char receive_buffer[kReceiveBufferSize]; + while (tcp_running_.load()) { + const ssize_t received = ::recv(client_fd, receive_buffer, sizeof(receive_buffer), 0); + if (received <= 0) { + break; + } + buffer.append(receive_buffer, static_cast(received)); + + while (true) { + const size_t start_pos = buffer.find(" 32 * 1024) { + buffer.clear(); + } + break; + } + const size_t end_pos = buffer.find("", start_pos); + if (end_pos == std::string::npos) { + if (start_pos > 0) { + buffer.erase(0, start_pos); + } + break; + } + + const size_t event_end = end_pos + std::string("").size(); + const std::string xml = buffer.substr(start_pos, event_end - start_pos); + buffer.erase(0, event_end); + + CotObservation observation; + if (!parseCotXml(xml, observation)) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "Failed to parse COT XML from TCP client %s", peer_name.c_str()); + continue; + } + + TruckState state; + { + std::lock_guard lock(state_mutex_); + auto &entry = trucks_[observation.truck_id]; + entry.distance_along_trajectory_m = observation.distance_along_trajectory_m; + entry.lat = observation.lat; + entry.lon = observation.lon; + entry.speed_kmh = observation.speed_kmh; + entry.course_deg = observation.course_deg; + entry.tcp_connected = true; + entry.last_cot_stamp = now(); + state = entry; + } + + seen_uids.insert(observation.truck_id); + publishTruckState(observation.truck_id, state); + } + } + + for (const auto &uid : seen_uids) { + TruckState state; + bool found = false; + { + std::lock_guard lock(state_mutex_); + const auto it = trucks_.find(uid); + if (it != trucks_.end()) { + it->second.tcp_connected = false; + it->second.last_cot_stamp = now(); + state = it->second; + found = true; + } + } + if (found) { + publishTruckState(uid, state); + } + } + + ::shutdown(client_fd, SHUT_RDWR); + ::close(client_fd); + RCLCPP_INFO(get_logger(), "TruckObject TCP client disconnected: %s", peer_name.c_str()); +} + void TruckObjectControl::evaluateAndPublishSpeedCommand() { std::vector connected_positions; std::vector connected_ids; const auto now_time = now(); - for (const auto &[id, state] : trucks_) { - if (!state.tcp_connected || !isCotFresh(state, now_time)) { - continue; + { + std::lock_guard lock(state_mutex_); + for (const auto &[id, state] : trucks_) { + if (!state.tcp_connected || !isCotFresh(state, now_time)) { + continue; + } + connected_positions.push_back(state.distance_along_trajectory_m); + connected_ids.push_back(id); } - connected_positions.push_back(state.distance_along_trajectory_m); - connected_ids.push_back(id); } if (connected_positions.size() < 2) { @@ -151,6 +549,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { last_published_speed_kmh_ = target_speed_kmh; RCLCPP_WARN(get_logger(), - "Published speed command %.1f km/h (reason=%s, min_gap=%.2f m, connected=%zu)", - target_speed_kmh, reason.c_str(), min_gap_m, connected_ids.size()); + "Published speed command %.1f km/h (reason=%s, min_gap=%.2f m, connected=%zu, ids=%s)", + target_speed_kmh, reason.c_str(), min_gap_m, connected_ids.size(), + connected_ids.empty() ? "-" : connected_ids.front().c_str()); } diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index b60e21266..e8736bece 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] = {} def main() -> None: @@ -26,14 +34,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: @@ -43,13 +123,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: @@ -59,6 +194,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") @@ -69,12 +205,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 83fdf907e..27841d20f 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, @@ -25,4 +28,4 @@ 'truck_object_gui = atos_gui.main:main' ], }, -) \ No newline at end of file +) 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 78505e6c30dbb24c39193708507f2d4bd47e4083 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Wed, 15 Apr 2026 08:32:16 +0200 Subject: [PATCH 06/29] Simulate with 3 trucks --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 2525 -> 22888 bytes atos/launch/launch_atosfleetmanagement.py | 37 +++++++++- .../inc/atostrucksimulator.hpp | 3 +- .../src/atostrucksimulator.cpp | 15 +++- atos_gui/atos_gui/static/fleet_map.js | 67 +++++++++++++++++- 5 files changed, 116 insertions(+), 6 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/modules/TruckObjectControl/inc/atostrucksimulator.hpp b/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp index bcac53350..f9f03cea7 100644 --- a/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp +++ b/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp @@ -29,9 +29,10 @@ class AtosTruckSimulator : public rclcpp::Node { int start_index_ = 0; double initial_speed_kmh_ = 0.0; double target_speed_kmh_ = 40.0; - double acceleration_mps2_ = 0.8; + double acceleration_mps2_ = 2.0; double publish_hz_ = 5.0; bool loop_path_ = true; + bool ignore_warning_speed_commands_ = false; double current_distance_m_ = 0.0; double current_speed_mps_ = 0.0; diff --git a/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp index d47a51a63..f7df779a3 100644 --- a/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp +++ b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp @@ -55,6 +55,7 @@ AtosTruckSimulator::AtosTruckSimulator() : Node("atos_truck_simulator") { declare_parameter("acceleration_mps2", acceleration_mps2_); declare_parameter("publish_hz", publish_hz_); declare_parameter("loop_path", loop_path_); + declare_parameter("ignore_warning_speed_commands", ignore_warning_speed_commands_); uid_ = get_parameter("uid").as_string(); tcp_host_ = get_parameter("tcp_host").as_string(); @@ -66,6 +67,7 @@ AtosTruckSimulator::AtosTruckSimulator() : Node("atos_truck_simulator") { acceleration_mps2_ = std::max(0.01, get_parameter("acceleration_mps2").as_double()); publish_hz_ = std::max(1.0, get_parameter("publish_hz").as_double()); loop_path_ = get_parameter("loop_path").as_bool(); + ignore_warning_speed_commands_ = get_parameter("ignore_warning_speed_commands").as_bool(); if (!loadTrajectoryPath()) { RCLCPP_ERROR(get_logger(), "Failed to load trajectory at '%s'", trajectory_geojson_path_.c_str()); @@ -92,8 +94,9 @@ AtosTruckSimulator::AtosTruckSimulator() : Node("atos_truck_simulator") { std::bind(&AtosTruckSimulator::simulationStep, this)); RCLCPP_INFO(get_logger(), - "AtosTruckSimulator started (uid=%s, start_index=%d, target=%.1f km/h, accel=%.2f m/s^2, tcp=%s:%d)", - uid_.c_str(), start_index_, target_speed_kmh_, acceleration_mps2_, tcp_host_.c_str(), tcp_port_); + "AtosTruckSimulator started (uid=%s, start_index=%d, target=%.1f km/h, accel=%.2f m/s^2, tcp=%s:%d, ignore_warning=%s)", + uid_.c_str(), start_index_, target_speed_kmh_, acceleration_mps2_, tcp_host_.c_str(), tcp_port_, + ignore_warning_speed_commands_ ? "true" : "false"); } bool AtosTruckSimulator::loadTrajectoryPath() { @@ -318,7 +321,13 @@ void AtosTruckSimulator::onSpeedCommand(const std_msgs::msg::String::SharedPtr m const auto value_end = msg->data.find(';', value_start); const auto value = msg->data.substr(value_start, value_end - value_start); try { - target_speed_kmh_ = std::max(0.0, std::stod(value)); + const double commanded_speed_kmh = std::max(0.0, std::stod(value)); + // Keep this simulator at its cruise speed through first-limit warning commands, + // but still obey full-stop commands at the second limit. + if (ignore_warning_speed_commands_ && commanded_speed_kmh > 0.0) { + return; + } + target_speed_kmh_ = commanded_speed_kmh; } catch (...) { return; } 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 d5293583381eb0ac13779b0252d34f0b82f974e0 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Wed, 15 Apr 2026 08:45:56 +0200 Subject: [PATCH 07/29] removed user path dependencies --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 22888 -> 22882 bytes .../modules/TruckObjectControl/CMakeLists.txt | 3 ++ .../inc/atostrucksimulator.hpp | 2 +- .../inc/truckobjectcontrol.hpp | 2 +- .../src/atostrucksimulator.cpp | 35 ++++++++++++++++++ .../src/truckobjectcontrol.cpp | 35 ++++++++++++++++++ 6 files changed, 75 insertions(+), 2 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 + #include #include #include @@ -13,7 +15,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -23,6 +27,7 @@ using json = nlohmann::json; namespace { constexpr double kEpsilon = 1e-9; +constexpr const char *kGeoJsonName = "RuralRoad_center_of_driving_lane_ccw.geojson"; double degToRad(double value) { return value * M_PI / 180.0; @@ -42,6 +47,35 @@ double geodesicDistanceMeters(double lat1, double lon1, double lat2, double lon2 const double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a)); return earth_radius_m * c; } + +std::string resolveTrajectoryPath(const std::string &configured_path) { + namespace fs = std::filesystem; + if (!configured_path.empty() && fs::exists(configured_path)) { + return configured_path; + } + + std::vector candidates; + candidates.emplace_back(fs::current_path() / "conf" / "conf" / kGeoJsonName); + candidates.emplace_back(fs::current_path() / ".." / "conf" / "conf" / kGeoJsonName); + + if (const char *home = std::getenv("HOME")) { + candidates.emplace_back(fs::path(home) / "atos_ws" / "src" / "atos" / "conf" / "conf" / kGeoJsonName); + candidates.emplace_back(fs::path(home) / "Documents" / "repos" / "ATOS" / "conf" / "conf" / kGeoJsonName); + } + + try { + const auto prefix = fs::path(ament_index_cpp::get_package_prefix("atos")); + candidates.emplace_back(prefix / "etc" / "conf" / kGeoJsonName); + } catch (...) { + } + + for (const auto &candidate : candidates) { + if (fs::exists(candidate)) { + return candidate.string(); + } + } + return configured_path; +} } AtosTruckSimulator::AtosTruckSimulator() : Node("atos_truck_simulator") { @@ -61,6 +95,7 @@ AtosTruckSimulator::AtosTruckSimulator() : Node("atos_truck_simulator") { tcp_host_ = get_parameter("tcp_host").as_string(); tcp_port_ = get_parameter("tcp_port").as_int(); trajectory_geojson_path_ = get_parameter("trajectory_geojson_path").as_string(); + trajectory_geojson_path_ = resolveTrajectoryPath(trajectory_geojson_path_); start_index_ = get_parameter("start_index").as_int(); initial_speed_kmh_ = get_parameter("initial_speed_kmh").as_double(); target_speed_kmh_ = get_parameter("target_speed_kmh").as_double(); diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 7be548f12..7d1ab59bc 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -5,6 +5,8 @@ */ #include "truckobjectcontrol.hpp" +#include + #include #include #include @@ -13,6 +15,8 @@ #include #include #include +#include +#include #include #include #include @@ -27,6 +31,7 @@ using json = nlohmann::json; namespace { constexpr size_t kReceiveBufferSize = 4096; constexpr int kAcceptPollSleepMs = 100; +constexpr const char *kGeoJsonName = "RuralRoad_center_of_driving_lane_ccw.geojson"; double degToRad(double value) { return value * M_PI / 180.0; @@ -42,6 +47,35 @@ double geodesicDistanceMeters(double lat1, double lon1, double lat2, double lon2 const double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a)); return earth_radius_m * c; } + +std::string resolveTrajectoryPath(const std::string &configured_path) { + namespace fs = std::filesystem; + if (!configured_path.empty() && fs::exists(configured_path)) { + return configured_path; + } + + std::vector candidates; + candidates.emplace_back(fs::current_path() / "conf" / "conf" / kGeoJsonName); + candidates.emplace_back(fs::current_path() / ".." / "conf" / "conf" / kGeoJsonName); + + if (const char *home = std::getenv("HOME")) { + candidates.emplace_back(fs::path(home) / "atos_ws" / "src" / "atos" / "conf" / "conf" / kGeoJsonName); + candidates.emplace_back(fs::path(home) / "Documents" / "repos" / "ATOS" / "conf" / "conf" / kGeoJsonName); + } + + try { + const auto prefix = fs::path(ament_index_cpp::get_package_prefix("atos")); + candidates.emplace_back(prefix / "etc" / "conf" / kGeoJsonName); + } catch (...) { + } + + for (const auto &candidate : candidates) { + if (fs::exists(candidate)) { + return candidate.string(); + } + } + return configured_path; +} } // namespace TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { @@ -62,6 +96,7 @@ TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { cot_tcp_port_ = get_parameter("cot_tcp_port").as_int(); cot_tcp_bind_address_ = get_parameter("cot_tcp_bind_address").as_string(); trajectory_geojson_path_ = get_parameter("trajectory_geojson_path").as_string(); + trajectory_geojson_path_ = resolveTrajectoryPath(trajectory_geojson_path_); cot_sub_ = create_subscription( "truck_objects/cot", 50, std::bind(&TruckObjectControl::onCotMessage, this, _1)); From ba75287789d46272145e9e39d2adf2102a1ce9d1 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Thu, 16 Apr 2026 09:38:52 +0200 Subject: [PATCH 08/29] Docker container supported --- Dockerfile | 7 +- README.md | 53 +++++- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 22882 -> 24192 bytes .../inc/truckobjectcontrol.hpp | 6 +- .../src/truckobjectcontrol.cpp | 154 ++++++++++++------ atos_gui/atos_gui/main.py | 7 +- 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 +++ 11 files changed, 235 insertions(+), 64 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/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index 69dea99eb..7190f4658 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -31,7 +32,7 @@ class TruckObjectControl : public rclcpp::Node { double distance_along_trajectory_m = 0.0; double lat = 0.0; double lon = 0.0; - double speed_kmh = 0.0; + double speed_mps = 0.0; double course_deg = 0.0; bool tcp_connected = false; rclcpp::Time last_cot_stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); @@ -42,7 +43,7 @@ class TruckObjectControl : public rclcpp::Node { double distance_along_trajectory_m = 0.0; double lat = 0.0; double lon = 0.0; - double speed_kmh = 0.0; + double speed_mps = 0.0; double course_deg = 0.0; bool tcp_connected = false; }; @@ -72,6 +73,7 @@ class TruckObjectControl : public rclcpp::Node { std::thread tcp_accept_thread_; std::vector tcp_client_threads_; std::mutex tcp_threads_mutex_; + std::set tcp_client_fds_; void onCotMessage(const std_msgs::msg::String::SharedPtr msg); void evaluateAndPublishSpeedCommand(); diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 7d1ab59bc..073571f5a 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -8,8 +8,10 @@ #include #include +#include #include #include +#include #include #include @@ -133,7 +135,8 @@ void TruckObjectControl::publishTruckState(const std::string &truck_id, const Tr payload["distance_m"] = state.distance_along_trajectory_m; payload["lat"] = state.lat; payload["lon"] = state.lon; - payload["speed_kmh"] = state.speed_kmh; + payload["speed_mps"] = state.speed_mps; + payload["speed_kmh"] = state.speed_mps * 3.6; payload["course_deg"] = state.course_deg; payload["tcp_connected"] = state.tcp_connected; payload["stamp_sec"] = now().seconds(); @@ -158,7 +161,7 @@ void TruckObjectControl::onCotMessage(const std_msgs::msg::String::SharedPtr msg entry.distance_along_trajectory_m = observation.distance_along_trajectory_m; entry.lat = observation.lat; entry.lon = observation.lon; - entry.speed_kmh = observation.speed_kmh; + entry.speed_mps = observation.speed_mps; entry.course_deg = observation.course_deg; entry.tcp_connected = observation.tcp_connected; entry.last_cot_stamp = now(); @@ -202,8 +205,11 @@ bool TruckObjectControl::parseCotPlaceholder(const std::string &payload, CotObse out.lat = std::stod(fields.at("lat")); out.lon = std::stod(fields.at("lon")); } - if (fields.find("speed_kmh") != fields.end()) { - out.speed_kmh = std::stod(fields.at("speed_kmh")); + if (fields.find("speed_mps") != fields.end()) { + out.speed_mps = std::stod(fields.at("speed_mps")); + } else if (fields.find("speed_kmh") != fields.end()) { + // Backward compatibility for older placeholder publishers. + out.speed_mps = std::stod(fields.at("speed_kmh")) / 3.6; } if (fields.find("course_deg") != fields.end()) { out.course_deg = std::stod(fields.at("course_deg")); @@ -242,16 +248,15 @@ bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation return false; } - out.speed_kmh = 0.0; + out.speed_mps = 0.0; out.course_deg = 0.0; if (std::regex_search(payload, m, track_re) && m.size() >= 3) { try { - // The incoming example uses speed in m/s; convert to km/h for command logic and UI. - const double speed_ms = std::stod(m[1].str()); - out.speed_kmh = speed_ms * 3.6; + // CoT track speed is in m/s. + out.speed_mps = std::stod(m[1].str()); out.course_deg = std::stod(m[2].str()); } catch (...) { - out.speed_kmh = 0.0; + out.speed_mps = 0.0; out.course_deg = 0.0; } } @@ -367,6 +372,7 @@ void TruckObjectControl::startTcpServer() { int enable = 1; (void)setsockopt(tcp_server_fd_, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)); + (void)setsockopt(tcp_server_fd_, SOL_SOCKET, SO_KEEPALIVE, &enable, sizeof(enable)); sockaddr_in addr{}; addr.sin_family = AF_INET; @@ -411,6 +417,15 @@ void TruckObjectControl::stopTcpServer() { tcp_accept_thread_.join(); } + { + std::lock_guard lock(tcp_threads_mutex_); + for (const int fd : tcp_client_fds_) { + ::shutdown(fd, SHUT_RDWR); + ::close(fd); + } + tcp_client_fds_.clear(); + } + std::lock_guard lock(tcp_threads_mutex_); for (auto &thread : tcp_client_threads_) { if (thread.joinable()) { @@ -427,12 +442,23 @@ void TruckObjectControl::acceptTcpClients() { const int client_fd = ::accept(tcp_server_fd_, reinterpret_cast(&client_addr), &client_len); if (client_fd < 0) { - if (tcp_running_.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(kAcceptPollSleepMs)); + if (!tcp_running_.load()) { + break; } + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, + "TCP accept failed (errno=%d). Listener will continue.", errno); + std::this_thread::sleep_for(std::chrono::milliseconds(kAcceptPollSleepMs)); continue; } + int keepalive = 1; + (void)setsockopt(client_fd, SOL_SOCKET, SO_KEEPALIVE, &keepalive, sizeof(keepalive)); + + timeval recv_timeout{}; + recv_timeout.tv_sec = 1; + recv_timeout.tv_usec = 0; + (void)setsockopt(client_fd, SOL_SOCKET, SO_RCVTIMEO, &recv_timeout, sizeof(recv_timeout)); + char ip_buf[INET_ADDRSTRLEN] = {0}; const char *peer_ip = ::inet_ntop(AF_INET, &client_addr.sin_addr, ip_buf, sizeof(ip_buf)); std::ostringstream peer; @@ -441,6 +467,7 @@ void TruckObjectControl::acceptTcpClients() { RCLCPP_INFO(get_logger(), "Accepted TruckObject TCP client %s", peer.str().c_str()); std::lock_guard lock(tcp_threads_mutex_); + tcp_client_fds_.insert(client_fd); tcp_client_threads_.emplace_back(&TruckObjectControl::handleTcpClient, this, client_fd, peer.str()); } } @@ -450,58 +477,73 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ buffer.reserve(8 * 1024); std::set seen_uids; - char receive_buffer[kReceiveBufferSize]; - while (tcp_running_.load()) { - const ssize_t received = ::recv(client_fd, receive_buffer, sizeof(receive_buffer), 0); - if (received <= 0) { - break; - } - buffer.append(receive_buffer, static_cast(received)); - - while (true) { - const size_t start_pos = buffer.find(" 32 * 1024) { - buffer.clear(); - } - break; + try { + char receive_buffer[kReceiveBufferSize]; + while (tcp_running_.load()) { + const ssize_t received = ::recv(client_fd, receive_buffer, sizeof(receive_buffer), 0); + if (received == 0) { + break; // peer closed } - const size_t end_pos = buffer.find("", start_pos); - if (end_pos == std::string::npos) { - if (start_pos > 0) { - buffer.erase(0, start_pos); + if (received < 0) { + if (errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK) { + continue; } + RCLCPP_WARN(get_logger(), + "TCP receive error from %s (errno=%d). Closing this connection only.", + peer_name.c_str(), errno); break; } - const size_t event_end = end_pos + std::string("").size(); - const std::string xml = buffer.substr(start_pos, event_end - start_pos); - buffer.erase(0, event_end); + buffer.append(receive_buffer, static_cast(received)); - CotObservation observation; - if (!parseCotXml(xml, observation)) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, - "Failed to parse COT XML from TCP client %s", peer_name.c_str()); - continue; - } + while (true) { + const size_t start_pos = buffer.find(" 32 * 1024) { + buffer.clear(); + } + break; + } + const size_t end_pos = buffer.find("", start_pos); + if (end_pos == std::string::npos) { + if (start_pos > 0) { + buffer.erase(0, start_pos); + } + break; + } - TruckState state; - { - std::lock_guard lock(state_mutex_); - auto &entry = trucks_[observation.truck_id]; - entry.distance_along_trajectory_m = observation.distance_along_trajectory_m; - entry.lat = observation.lat; - entry.lon = observation.lon; - entry.speed_kmh = observation.speed_kmh; - entry.course_deg = observation.course_deg; - entry.tcp_connected = true; - entry.last_cot_stamp = now(); - state = entry; - } + const size_t event_end = end_pos + std::string("").size(); + const std::string xml = buffer.substr(start_pos, event_end - start_pos); + buffer.erase(0, event_end); + + CotObservation observation; + if (!parseCotXml(xml, observation)) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "Failed to parse COT XML from TCP client %s", peer_name.c_str()); + continue; + } - seen_uids.insert(observation.truck_id); - publishTruckState(observation.truck_id, state); + TruckState state; + { + std::lock_guard lock(state_mutex_); + auto &entry = trucks_[observation.truck_id]; + entry.distance_along_trajectory_m = observation.distance_along_trajectory_m; + entry.lat = observation.lat; + entry.lon = observation.lon; + entry.speed_mps = observation.speed_mps; + entry.course_deg = observation.course_deg; + entry.tcp_connected = true; + entry.last_cot_stamp = now(); + state = entry; + } + + seen_uids.insert(observation.truck_id); + publishTruckState(observation.truck_id, state); + } } + } catch (...) { + RCLCPP_WARN(get_logger(), "Unexpected exception while handling client %s. Connection will be closed.", + peer_name.c_str()); } for (const auto &uid : seen_uids) { @@ -522,6 +564,10 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ } } + { + std::lock_guard lock(tcp_threads_mutex_); + tcp_client_fds_.erase(client_fd); + } ::shutdown(client_fd, SHUT_RDWR); ::close(client_fd); RCLCPP_INFO(get_logger(), "TruckObject TCP client disconnected: %s", peer_name.c_str()); diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index e8736bece..bdb3deb61 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,4 +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 0ebbf88ec2bf06e9f67a26d3f037598b747aa723 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Thu, 16 Apr 2026 09:47:34 +0200 Subject: [PATCH 09/29] 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/29] trying to build docker image --- scripts/installation/install_deps.sh | 44 +++++++++++++++++++++++++--- 1 file changed, 40 insertions(+), 4 deletions(-) diff --git a/scripts/installation/install_deps.sh b/scripts/installation/install_deps.sh index cc3647bfc..79ba25a23 100755 --- a/scripts/installation/install_deps.sh +++ b/scripts/installation/install_deps.sh @@ -19,10 +19,44 @@ fi source "${ATOS_REPO_PATH}/scripts/installation/install_functions.sh" +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 +} + # 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 python3 -m pip install -r ${ATOS_REPO_PATH}/scripts/installation/requirements.txt # Check if apt failed to install dependencies @@ -38,7 +72,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 \ @@ -52,7 +87,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 @@ -121,4 +157,4 @@ fi # Remove custom path for source installation echo "Removing custom path for source installation..." sudo rm -rf $SOURCE_PATH -check_command_failed $? "Failed to remove ${SOURCE_PATH} path for source installation." \ No newline at end of file +check_command_failed $? "Failed to remove ${SOURCE_PATH} path for source installation." From 09a15a6761515805e1c0f6da5daeaa3d8dcea68c Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Thu, 16 Apr 2026 14:20:06 +0200 Subject: [PATCH 11/29] handling COT->path_name and send back request/info to pf on ctp --- .../inc/atostrucksimulator.hpp | 7 +- .../inc/truckobjectcontrol.hpp | 20 +- .../src/atostrucksimulator.cpp | 112 +++++-- .../src/truckobjectcontrol.cpp | 284 +++++++++++++++--- atos_gui/atos_gui/main.py | 100 ++++-- atos_gui/atos_gui/static/fleet_map.js | 115 +++++-- 6 files changed, 517 insertions(+), 121 deletions(-) diff --git a/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp b/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp index 32d3216a0..aefd643ac 100644 --- a/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp +++ b/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp @@ -26,6 +26,7 @@ class AtosTruckSimulator : public rclcpp::Node { std::string tcp_host_ = "127.0.0.1"; int tcp_port_ = 8114; std::string trajectory_geojson_path_ = ""; + std::string trajectory_path_name_ = ""; int start_index_ = 0; double initial_speed_kmh_ = 0.0; double target_speed_kmh_ = 40.0; @@ -38,6 +39,7 @@ class AtosTruckSimulator : public rclcpp::Node { double current_speed_mps_ = 0.0; int tcp_fd_ = -1; + std::string tcp_rx_buffer_; std::vector trajectory_path_; rclcpp::TimerBase::SharedPtr simulation_timer_; @@ -48,8 +50,11 @@ class AtosTruckSimulator : public rclcpp::Node { bool loadTrajectoryPath(); bool ensureTcpConnected(); void closeTcp(); + void pollTcpCommands(); void simulationStep(); - bool pointAtDistance(double distance_m, double &lat, double &lon, double &course_deg) const; + bool pointAtDistance(double distance_m, double &lat, double &lon, double &course_deg, + int &path_index) const; static std::string utcIso8601FromRosTime(const rclcpp::Time &time); + void applySpeedCommandPayload(const std::string &payload); void onSpeedCommand(const std_msgs::msg::String::SharedPtr msg); }; diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index 7190f4658..62f4a5bab 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -35,6 +35,8 @@ class TruckObjectControl : public rclcpp::Node { double speed_mps = 0.0; double course_deg = 0.0; bool tcp_connected = false; + std::string path_name = ""; + int path_index = -1; rclcpp::Time last_cot_stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); }; @@ -46,6 +48,8 @@ class TruckObjectControl : public rclcpp::Node { double speed_mps = 0.0; double course_deg = 0.0; bool tcp_connected = false; + std::string path_name = ""; + int path_index = -1; }; rclcpp::Subscription::SharedPtr cot_sub_; @@ -56,6 +60,8 @@ class TruckObjectControl : public rclcpp::Node { std::mutex state_mutex_; std::unordered_map trucks_; std::vector trajectory_path_; + std::unordered_map> trajectory_cache_; + std::mutex trajectory_cache_mutex_; double warning_distance_m_ = 400.0; double stop_distance_m_ = 200.0; @@ -65,8 +71,9 @@ class TruckObjectControl : public rclcpp::Node { int cot_tcp_port_ = 8114; std::string cot_tcp_bind_address_ = "0.0.0.0"; std::string trajectory_geojson_path_ = ""; + std::string default_path_name_ = ""; - double last_published_speed_kmh_ = -1.0; + double last_published_speed_mps_ = -1.0; std::atomic tcp_running_{false}; int tcp_server_fd_ = -1; @@ -74,19 +81,26 @@ class TruckObjectControl : public rclcpp::Node { std::vector tcp_client_threads_; std::mutex tcp_threads_mutex_; std::set tcp_client_fds_; + std::mutex tcp_command_mutex_; + std::unordered_map uid_to_client_fd_; void onCotMessage(const std_msgs::msg::String::SharedPtr msg); void evaluateAndPublishSpeedCommand(); void publishTruckState(const std::string &truck_id, const TruckState &state); bool parseCotPlaceholder(const std::string &payload, CotObservation &out) const; - bool parseCotXml(const std::string &payload, CotObservation &out) const; + bool parseCotXml(const std::string &payload, CotObservation &out); bool isCotFresh(const TruckState &state, const rclcpp::Time &now) const; bool loadTrajectoryPath(); - double projectDistanceAlongTrajectory(double lat, double lon) const; + bool loadTrajectoryPathFromFile(const std::string &path, std::vector &out) const; + std::string resolveTrajectoryPathByName(const std::string &path_name) const; + const std::vector *getTrajectoryForPath(const std::string &path_name); + double projectDistanceAlongTrajectory(double lat, double lon, + const std::vector *trajectory = nullptr) const; void startTcpServer(); void stopTcpServer(); void acceptTcpClients(); void handleTcpClient(int client_fd, const std::string &peer_name); + void sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command); }; diff --git a/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp index 184183772..ed825c51d 100644 --- a/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp +++ b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp @@ -8,6 +8,8 @@ #include #include +#include +#include #include #include #include @@ -21,6 +23,7 @@ #include #include #include +#include #include using json = nlohmann::json; @@ -96,6 +99,7 @@ AtosTruckSimulator::AtosTruckSimulator() : Node("atos_truck_simulator") { tcp_port_ = get_parameter("tcp_port").as_int(); trajectory_geojson_path_ = get_parameter("trajectory_geojson_path").as_string(); trajectory_geojson_path_ = resolveTrajectoryPath(trajectory_geojson_path_); + trajectory_path_name_ = std::filesystem::path(trajectory_geojson_path_).filename().string(); start_index_ = get_parameter("start_index").as_int(); initial_speed_kmh_ = get_parameter("initial_speed_kmh").as_double(); target_speed_kmh_ = get_parameter("target_speed_kmh").as_double(); @@ -129,9 +133,9 @@ AtosTruckSimulator::AtosTruckSimulator() : Node("atos_truck_simulator") { std::bind(&AtosTruckSimulator::simulationStep, this)); RCLCPP_INFO(get_logger(), - "AtosTruckSimulator started (uid=%s, start_index=%d, target=%.1f km/h, accel=%.2f m/s^2, tcp=%s:%d, ignore_warning=%s)", - uid_.c_str(), start_index_, target_speed_kmh_, acceleration_mps2_, tcp_host_.c_str(), tcp_port_, - ignore_warning_speed_commands_ ? "true" : "false"); + "AtosTruckSimulator started (uid=%s, path=%s, start_index=%d, target=%.1f km/h, accel=%.2f m/s^2, tcp=%s:%d, ignore_warning=%s)", + uid_.c_str(), trajectory_path_name_.c_str(), start_index_, target_speed_kmh_, acceleration_mps2_, + tcp_host_.c_str(), tcp_port_, ignore_warning_speed_commands_ ? "true" : "false"); } bool AtosTruckSimulator::loadTrajectoryPath() { @@ -229,6 +233,11 @@ bool AtosTruckSimulator::ensureTcpConnected() { return false; } + const int flags = fcntl(tcp_fd_, F_GETFL, 0); + if (flags >= 0) { + (void)fcntl(tcp_fd_, F_SETFL, flags | O_NONBLOCK); + } + return true; } @@ -237,6 +246,7 @@ void AtosTruckSimulator::closeTcp() { ::shutdown(tcp_fd_, SHUT_RDWR); ::close(tcp_fd_); tcp_fd_ = -1; + tcp_rx_buffer_.clear(); } } @@ -249,7 +259,8 @@ std::string AtosTruckSimulator::utcIso8601FromRosTime(const rclcpp::Time &time) return out.str(); } -bool AtosTruckSimulator::pointAtDistance(double distance_m, double &lat, double &lon, double &course_deg) const { +bool AtosTruckSimulator::pointAtDistance(double distance_m, double &lat, double &lon, double &course_deg, + int &path_index) const { if (trajectory_path_.size() < 2) { return false; } @@ -289,6 +300,8 @@ bool AtosTruckSimulator::pointAtDistance(double distance_m, double &lat, double std::sin(degToRad(a.lat)) * std::cos(degToRad(b.lat)) * std::cos(degToRad(b.lon - a.lon)); course_deg = std::fmod(radToDeg(std::atan2(y, x)) + 360.0, 360.0); + + path_index = static_cast(t < 0.5 ? (segment_index - 1) : segment_index); return true; } @@ -310,7 +323,8 @@ void AtosTruckSimulator::simulationStep() { double lat = 0.0; double lon = 0.0; double course_deg = 0.0; - if (!pointAtDistance(current_distance_m_, lat, lon, course_deg)) { + int path_index = 0; + if (!pointAtDistance(current_distance_m_, lat, lon, course_deg, path_index)) { return; } @@ -319,6 +333,7 @@ void AtosTruckSimulator::simulationStep() { tcp_host_.c_str(), tcp_port_); return; } + pollTcpCommands(); const std::string time_str = utcIso8601FromRosTime(now_time); const std::string stale_str = utcIso8601FromRosTime(now_time + rclcpp::Duration::from_seconds(10.0)); @@ -331,7 +346,8 @@ void AtosTruckSimulator::simulationStep() { << "\" hae=\"10\" ce=\"5\" le=\"5\"/><__group name=\"Dark Green\" role=\"Test Vehicle\"/>"; + << current_speed_mps_ << "\" course=\"" << course_deg << "\"/>"; const std::string payload = cot.str(); const ssize_t sent = ::send(tcp_fd_, payload.data(), payload.size(), MSG_NOSIGNAL); @@ -342,28 +358,80 @@ void AtosTruckSimulator::simulationStep() { } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, - "Sim uid=%s distance=%.1f m speed=%.1f km/h target=%.1f km/h lat=%.7f lon=%.7f", - uid_.c_str(), current_distance_m_, current_speed_mps_ * 3.6, target_speed_kmh_, lat, lon); + "Sim uid=%s path=%s idx=%d distance=%.1f m speed=%.1f km/h target=%.1f km/h lat=%.7f lon=%.7f", + uid_.c_str(), trajectory_path_name_.c_str(), path_index, current_distance_m_, + current_speed_mps_ * 3.6, target_speed_kmh_, lat, lon); } void AtosTruckSimulator::onSpeedCommand(const std_msgs::msg::String::SharedPtr msg) { - const auto key = std::string("target_speed_kmh="); - const auto pos = msg->data.find(key); - if (pos == std::string::npos) { + applySpeedCommandPayload(msg->data); +} + +void AtosTruckSimulator::applySpeedCommandPayload(const std::string &payload) { + auto parseField = [&](const std::string &key) -> std::optional { + const auto pos = payload.find(key); + if (pos == std::string::npos) { + return std::nullopt; + } + const auto value_start = pos + key.size(); + const auto value_end = payload.find(';', value_start); + const auto value = payload.substr(value_start, value_end - value_start); + try { + return std::stod(value); + } catch (...) { + return std::nullopt; + } + }; + + double commanded_speed_kmh = 0.0; + if (const auto speed_mps = parseField("target_speed_mps="); speed_mps.has_value()) { + commanded_speed_kmh = std::max(0.0, *speed_mps) * 3.6; + } else if (const auto speed_kmh = parseField("target_speed_kmh="); speed_kmh.has_value()) { + commanded_speed_kmh = std::max(0.0, *speed_kmh); + } else { return; } - const auto value_start = pos + key.size(); - const auto value_end = msg->data.find(';', value_start); - const auto value = msg->data.substr(value_start, value_end - value_start); - try { - const double commanded_speed_kmh = std::max(0.0, std::stod(value)); - // Keep this simulator at its cruise speed through first-limit warning commands, - // but still obey full-stop commands at the second limit. - if (ignore_warning_speed_commands_ && commanded_speed_kmh > 0.0) { + + if (ignore_warning_speed_commands_ && commanded_speed_kmh > 0.0) { + return; + } + target_speed_kmh_ = commanded_speed_kmh; +} + +void AtosTruckSimulator::pollTcpCommands() { + if (tcp_fd_ < 0) { + return; + } + + char rx[1024]; + while (true) { + const ssize_t n = ::recv(tcp_fd_, rx, sizeof(rx), 0); + if (n == 0) { + closeTcp(); return; } - target_speed_kmh_ = commanded_speed_kmh; - } catch (...) { - return; + if (n < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) { + break; + } + closeTcp(); + return; + } + + tcp_rx_buffer_.append(rx, static_cast(n)); + while (true) { + const auto nl = tcp_rx_buffer_.find('\n'); + if (nl == std::string::npos) { + if (tcp_rx_buffer_.size() > 4096) { + tcp_rx_buffer_.clear(); + } + break; + } + const std::string line = tcp_rx_buffer_.substr(0, nl); + tcp_rx_buffer_.erase(0, nl + 1); + if (!line.empty()) { + applySpeedCommandPayload(line); + } + } } } diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 073571f5a..04228698b 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -33,7 +33,7 @@ using json = nlohmann::json; namespace { constexpr size_t kReceiveBufferSize = 4096; constexpr int kAcceptPollSleepMs = 100; -constexpr const char *kGeoJsonName = "RuralRoad_center_of_driving_lane_ccw.geojson"; +constexpr const char *kDefaultGeoJsonName = "RuralRoad_center_of_driving_lane_ccw.geojson"; double degToRad(double value) { return value * M_PI / 180.0; @@ -50,24 +50,24 @@ double geodesicDistanceMeters(double lat1, double lon1, double lat2, double lon2 return earth_radius_m * c; } -std::string resolveTrajectoryPath(const std::string &configured_path) { +std::string resolveDefaultTrajectoryPath(const std::string &configured_path) { namespace fs = std::filesystem; if (!configured_path.empty() && fs::exists(configured_path)) { return configured_path; } std::vector candidates; - candidates.emplace_back(fs::current_path() / "conf" / "conf" / kGeoJsonName); - candidates.emplace_back(fs::current_path() / ".." / "conf" / "conf" / kGeoJsonName); + candidates.emplace_back(fs::current_path() / "conf" / "conf" / kDefaultGeoJsonName); + candidates.emplace_back(fs::current_path() / ".." / "conf" / "conf" / kDefaultGeoJsonName); if (const char *home = std::getenv("HOME")) { - candidates.emplace_back(fs::path(home) / "atos_ws" / "src" / "atos" / "conf" / "conf" / kGeoJsonName); - candidates.emplace_back(fs::path(home) / "Documents" / "repos" / "ATOS" / "conf" / "conf" / kGeoJsonName); + candidates.emplace_back(fs::path(home) / "atos_ws" / "src" / "atos" / "conf" / "conf" / kDefaultGeoJsonName); + candidates.emplace_back(fs::path(home) / "Documents" / "repos" / "ATOS" / "conf" / "conf" / kDefaultGeoJsonName); } try { const auto prefix = fs::path(ament_index_cpp::get_package_prefix("atos")); - candidates.emplace_back(prefix / "etc" / "conf" / kGeoJsonName); + candidates.emplace_back(prefix / "etc" / "conf" / kDefaultGeoJsonName); } catch (...) { } @@ -98,7 +98,8 @@ TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { cot_tcp_port_ = get_parameter("cot_tcp_port").as_int(); cot_tcp_bind_address_ = get_parameter("cot_tcp_bind_address").as_string(); trajectory_geojson_path_ = get_parameter("trajectory_geojson_path").as_string(); - trajectory_geojson_path_ = resolveTrajectoryPath(trajectory_geojson_path_); + trajectory_geojson_path_ = resolveDefaultTrajectoryPath(trajectory_geojson_path_); + default_path_name_ = std::filesystem::path(trajectory_geojson_path_).filename().string(); cot_sub_ = create_subscription( "truck_objects/cot", 50, std::bind(&TruckObjectControl::onCotMessage, this, _1)); @@ -139,6 +140,8 @@ void TruckObjectControl::publishTruckState(const std::string &truck_id, const Tr payload["speed_kmh"] = state.speed_mps * 3.6; payload["course_deg"] = state.course_deg; payload["tcp_connected"] = state.tcp_connected; + payload["path_name"] = state.path_name; + payload["path_index"] = state.path_index; payload["stamp_sec"] = now().seconds(); std_msgs::msg::String msg; @@ -164,6 +167,8 @@ void TruckObjectControl::onCotMessage(const std_msgs::msg::String::SharedPtr msg entry.speed_mps = observation.speed_mps; entry.course_deg = observation.course_deg; entry.tcp_connected = observation.tcp_connected; + entry.path_name = observation.path_name; + entry.path_index = observation.path_index; entry.last_cot_stamp = now(); state = entry; } @@ -208,12 +213,17 @@ bool TruckObjectControl::parseCotPlaceholder(const std::string &payload, CotObse if (fields.find("speed_mps") != fields.end()) { out.speed_mps = std::stod(fields.at("speed_mps")); } else if (fields.find("speed_kmh") != fields.end()) { - // Backward compatibility for older placeholder publishers. out.speed_mps = std::stod(fields.at("speed_kmh")) / 3.6; } if (fields.find("course_deg") != fields.end()) { out.course_deg = std::stod(fields.at("course_deg")); } + if (fields.find("path_name") != fields.end()) { + out.path_name = fields.at("path_name"); + } + if (fields.find("path_index") != fields.end()) { + out.path_index = std::stoi(fields.at("path_index")); + } } catch (...) { return false; } @@ -221,12 +231,12 @@ bool TruckObjectControl::parseCotPlaceholder(const std::string &payload, CotObse return true; } -bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation &out) const { - // Expected input resembles: - // +bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation &out) { static const std::regex uid_re(R"re(uid="([^"]+)")re"); static const std::regex point_re(R"re(]*\blat="([^"]+)"[^>]*\blon="([^"]+)")re"); static const std::regex track_re(R"re(]*\bspeed="([^"]+)"[^>]*\bcourse="([^"]+)")re"); + static const std::regex path_name_re(R"re(]*\bpath_name="([^"]+)")re"); + static const std::regex path_index_re(R"re(]*\bpath_index="([^"]+)")re"); std::smatch m; if (!std::regex_search(payload, m, uid_re) || m.size() < 2) { @@ -252,7 +262,6 @@ bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation out.course_deg = 0.0; if (std::regex_search(payload, m, track_re) && m.size() >= 3) { try { - // CoT track speed is in m/s. out.speed_mps = std::stod(m[1].str()); out.course_deg = std::stod(m[2].str()); } catch (...) { @@ -261,7 +270,36 @@ bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation } } - out.distance_along_trajectory_m = projectDistanceAlongTrajectory(out.lat, out.lon); + out.path_name.clear(); + out.path_index = -1; + if (std::regex_search(payload, m, path_name_re) && m.size() >= 2) { + out.path_name = m[1].str(); + } + if (std::regex_search(payload, m, path_index_re) && m.size() >= 2) { + try { + out.path_index = std::stoi(m[1].str()); + } catch (...) { + out.path_index = -1; + } + } + + const std::vector *trajectory = nullptr; + if (!out.path_name.empty()) { + trajectory = getTrajectoryForPath(out.path_name); + } + if (!trajectory) { + trajectory = &trajectory_path_; + if (out.path_name.empty()) { + out.path_name = default_path_name_; + } + } + + if (trajectory && out.path_index >= 0 && static_cast(out.path_index) < trajectory->size()) { + out.distance_along_trajectory_m = (*trajectory)[static_cast(out.path_index)].distance_m; + } else { + out.distance_along_trajectory_m = projectDistanceAlongTrajectory(out.lat, out.lon, trajectory); + } + out.tcp_connected = true; return true; } @@ -271,10 +309,10 @@ bool TruckObjectControl::isCotFresh(const TruckState &state, const rclcpp::Time return age >= 0.0 && age <= cot_timeout_seconds_; } -bool TruckObjectControl::loadTrajectoryPath() { - trajectory_path_.clear(); +bool TruckObjectControl::loadTrajectoryPathFromFile(const std::string &path, std::vector &out) const { + out.clear(); - std::ifstream input(trajectory_geojson_path_); + std::ifstream input(path); if (!input.is_open()) { return false; } @@ -325,12 +363,9 @@ bool TruckObjectControl::loadTrajectoryPath() { if (!coord.is_array() || coord.size() < 2) { continue; } - const double lon = coord[0].get(); - const double lat = coord[1].get(); - GeoPoint p; - p.lat = lat; - p.lon = lon; + p.lon = coord[0].get(); + p.lat = coord[1].get(); p.distance_m = cumulative; if (has_prev) { @@ -338,22 +373,116 @@ bool TruckObjectControl::loadTrajectoryPath() { p.distance_m = cumulative; } - trajectory_path_.push_back(p); + out.push_back(p); prev = p; has_prev = true; } - return trajectory_path_.size() >= 2; + return out.size() >= 2; +} + +bool TruckObjectControl::loadTrajectoryPath() { + if (!loadTrajectoryPathFromFile(trajectory_geojson_path_, trajectory_path_)) { + return false; + } + + std::lock_guard lock(trajectory_cache_mutex_); + trajectory_cache_[default_path_name_] = trajectory_path_; + return true; +} + +std::string TruckObjectControl::resolveTrajectoryPathByName(const std::string &path_name) const { + namespace fs = std::filesystem; + + if (path_name.empty()) { + return trajectory_geojson_path_; + } + + const fs::path raw(path_name); + if (raw.is_absolute() && fs::exists(raw)) { + return raw.string(); + } + + const fs::path base_name = raw.filename(); + if (base_name.empty()) { + return ""; + } + + const fs::path configured = fs::path(trajectory_geojson_path_).parent_path() / base_name; + if (fs::exists(configured)) { + return configured.string(); + } + + std::vector candidates; + candidates.emplace_back(fs::current_path() / "conf" / "conf" / base_name); + candidates.emplace_back(fs::current_path() / ".." / "conf" / "conf" / base_name); + + if (const char *home = std::getenv("HOME")) { + candidates.emplace_back(fs::path(home) / "atos_ws" / "src" / "atos" / "conf" / "conf" / base_name); + candidates.emplace_back(fs::path(home) / "Documents" / "repos" / "ATOS" / "conf" / "conf" / base_name); + } + + try { + const auto prefix = fs::path(ament_index_cpp::get_package_prefix("atos")); + candidates.emplace_back(prefix / "etc" / "conf" / base_name); + } catch (...) { + } + + for (const auto &candidate : candidates) { + if (fs::exists(candidate)) { + return candidate.string(); + } + } + + return ""; +} + +const std::vector *TruckObjectControl::getTrajectoryForPath(const std::string &path_name) { + const std::string key = std::filesystem::path(path_name).filename().string(); + if (key.empty()) { + return nullptr; + } + + { + std::lock_guard lock(trajectory_cache_mutex_); + const auto it = trajectory_cache_.find(key); + if (it != trajectory_cache_.end()) { + return &it->second; + } + } + + const std::string resolved_path = resolveTrajectoryPathByName(key); + if (resolved_path.empty()) { + return nullptr; + } + + std::vector loaded; + if (!loadTrajectoryPathFromFile(resolved_path, loaded)) { + return nullptr; + } + + std::lock_guard lock(trajectory_cache_mutex_); + auto [it, inserted] = trajectory_cache_.emplace(key, std::move(loaded)); + if (inserted) { + RCLCPP_INFO(get_logger(), "Loaded trajectory '%s' with %zu points from %s", key.c_str(), it->second.size(), + resolved_path.c_str()); + } + return &it->second; } -double TruckObjectControl::projectDistanceAlongTrajectory(double lat, double lon) const { - if (trajectory_path_.empty()) { +double TruckObjectControl::projectDistanceAlongTrajectory(double lat, double lon, + const std::vector *trajectory) const { + const std::vector *path = trajectory; + if (!path || path->empty()) { + path = &trajectory_path_; + } + if (!path || path->empty()) { return 0.0; } double best_distance_to_path_m = std::numeric_limits::infinity(); double best_distance_along_m = 0.0; - for (const auto &p : trajectory_path_) { + for (const auto &p : *path) { const double d = geodesicDistanceMeters(lat, lon, p.lat, p.lon); if (d < best_distance_to_path_m) { best_distance_to_path_m = d; @@ -425,6 +554,10 @@ void TruckObjectControl::stopTcpServer() { } tcp_client_fds_.clear(); } + { + std::lock_guard lock(tcp_command_mutex_); + uid_to_client_fd_.clear(); + } std::lock_guard lock(tcp_threads_mutex_); for (auto &thread : tcp_client_threads_) { @@ -482,7 +615,7 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ while (tcp_running_.load()) { const ssize_t received = ::recv(client_fd, receive_buffer, sizeof(receive_buffer), 0); if (received == 0) { - break; // peer closed + break; } if (received < 0) { if (errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK) { @@ -533,9 +666,15 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ entry.speed_mps = observation.speed_mps; entry.course_deg = observation.course_deg; entry.tcp_connected = true; + entry.path_name = observation.path_name; + entry.path_index = observation.path_index; entry.last_cot_stamp = now(); state = entry; } + { + std::lock_guard lock(tcp_command_mutex_); + uid_to_client_fd_[observation.truck_id] = client_fd; + } seen_uids.insert(observation.truck_id); publishTruckState(observation.truck_id, state); @@ -562,6 +701,13 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ if (found) { publishTruckState(uid, state); } + { + std::lock_guard lock(tcp_command_mutex_); + const auto it = uid_to_client_fd_.find(uid); + if (it != uid_to_client_fd_.end() && it->second == client_fd) { + uid_to_client_fd_.erase(it); + } + } } { @@ -574,8 +720,12 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ } void TruckObjectControl::evaluateAndPublishSpeedCommand() { - std::vector connected_positions; - std::vector connected_ids; + struct ConnectedTruck { + std::string id; + double distance_m = 0.0; + int path_index = -1; + }; + std::vector connected; const auto now_time = now(); { @@ -584,53 +734,89 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { if (!state.tcp_connected || !isCotFresh(state, now_time)) { continue; } - connected_positions.push_back(state.distance_along_trajectory_m); - connected_ids.push_back(id); + connected.push_back(ConnectedTruck{id, state.distance_along_trajectory_m, state.path_index}); } } - if (connected_positions.size() < 2) { + if (connected.size() < 2) { return; } - std::sort(connected_positions.begin(), connected_positions.end()); + std::sort(connected.begin(), connected.end(), + [](const ConnectedTruck &a, const ConnectedTruck &b) { return a.distance_m < b.distance_m; }); double min_gap_m = std::numeric_limits::infinity(); - for (size_t i = 1; i < connected_positions.size(); ++i) { - min_gap_m = std::min(min_gap_m, std::fabs(connected_positions[i] - connected_positions[i - 1])); + for (size_t i = 1; i < connected.size(); ++i) { + min_gap_m = std::min(min_gap_m, std::fabs(connected[i].distance_m - connected[i - 1].distance_m)); } - double target_speed_kmh = -1.0; + double target_speed_mps = -1.0; std::string reason = "no_limit"; if (min_gap_m < stop_distance_m_) { - target_speed_kmh = stop_speed_kmh_; + target_speed_mps = stop_speed_kmh_ / 3.6; reason = "min_gap_below_stop_distance"; } else if (min_gap_m < warning_distance_m_) { - target_speed_kmh = warning_speed_kmh_; + target_speed_mps = warning_speed_kmh_ / 3.6; reason = "min_gap_below_warning_distance"; } - if (target_speed_kmh < 0.0) { + if (target_speed_mps < 0.0) { return; } - if (std::fabs(last_published_speed_kmh_ - target_speed_kmh) < 1e-6) { + if (std::fabs(last_published_speed_mps_ - target_speed_mps) < 1e-6) { return; } + // Keep ROS command as a global signal while TCP commands are individualized per truck. std_msgs::msg::String command; - command.data = "target_speed_kmh=" + std::to_string(target_speed_kmh) + - ";scope=all_connected_with_valid_tcp_and_fresh_cot" + - ";reason=" + reason + + command.data = "target_speed_mps=" + std::to_string(target_speed_mps) + + ";scope=all_connected_with_valid_tcp_and_fresh_cot" + ";reason=" + reason + ";min_gap_m=" + std::to_string(min_gap_m) + - ";connected_count=" + std::to_string(connected_ids.size()); + ";connected_count=" + std::to_string(connected.size()); speed_command_pub_->publish(command); - last_published_speed_kmh_ = target_speed_kmh; + for (size_t i = 0; i < connected.size(); ++i) { + const bool has_ahead = (i + 1) < connected.size(); + const std::string ahead_uid = has_ahead ? connected[i + 1].id : "none"; + const double ahead_gap = has_ahead ? (connected[i + 1].distance_m - connected[i].distance_m) : -1.0; + const int ahead_path_index = has_ahead ? connected[i + 1].path_index : -1; + + const std::string tcp_command = + "target_speed_mps=" + std::to_string(target_speed_mps) + ";distance_to_truck_ahead_m=" + + std::to_string(ahead_gap) + ";truck_ahead_path_index=" + std::to_string(ahead_path_index) + + ";truck_ahead_uid=" + ahead_uid + ";reason=" + reason + ";min_gap_m=" + std::to_string(min_gap_m) + + ";connected_count=" + std::to_string(connected.size()); + sendSpeedCommandToTcpClient(connected[i].id, tcp_command); + } + last_published_speed_mps_ = target_speed_mps; RCLCPP_WARN(get_logger(), - "Published speed command %.1f km/h (reason=%s, min_gap=%.2f m, connected=%zu, ids=%s)", - target_speed_kmh, reason.c_str(), min_gap_m, connected_ids.size(), - connected_ids.empty() ? "-" : connected_ids.front().c_str()); + "Published speed command %.2f m/s (reason=%s, min_gap=%.2f m, connected=%zu, first_id=%s)", + target_speed_mps, reason.c_str(), min_gap_m, connected.size(), + connected.empty() ? "-" : connected.front().id.c_str()); +} + +void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command) { + int target_fd = -1; + { + std::lock_guard lock(tcp_command_mutex_); + const auto it = uid_to_client_fd_.find(target_id); + if (it != uid_to_client_fd_.end()) { + target_fd = it->second; + } + } + + if (target_fd < 0) { + return; + } + + const std::string payload = command + "\n"; + const ssize_t sent = ::send(target_fd, payload.data(), payload.size(), MSG_NOSIGNAL); + if (sent < 0) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, + "Failed to send TCP speed command to uid=%s fd=%d (errno=%d)", target_id.c_str(), + target_fd, errno); + } } diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index bdb3deb61..784a82644 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] = {} def main() -> None: @@ -34,18 +35,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 @@ -61,20 +62,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 @@ -135,25 +184,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; @@ -163,7 +211,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) {{ @@ -182,7 +230,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 9966a989da37e9f4d391bd44e6be612c18530b0f Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Fri, 17 Apr 2026 14:33:22 +0200 Subject: [PATCH 12/29] tcp data to pf updated --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 24908 -> 20494 bytes .../inc/truckobjectcontrol.hpp | 2 - .../src/truckobjectcontrol.cpp | 55 ++++++++++-------- atos_gui/atos_gui/static/fleet_map.js | 4 +- 4 files changed, 32 insertions(+), 29 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 tcp_running_{false}; int tcp_server_fd_ = -1; std::thread tcp_accept_thread_; diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 04228698b..f4e8e3b11 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -738,7 +738,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { } } - if (connected.size() < 2) { + if (connected.empty()) { return; } @@ -749,30 +749,24 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { for (size_t i = 1; i < connected.size(); ++i) { min_gap_m = std::min(min_gap_m, std::fabs(connected[i].distance_m - connected[i - 1].distance_m)); } - - double target_speed_mps = -1.0; - std::string reason = "no_limit"; - - if (min_gap_m < stop_distance_m_) { - target_speed_mps = stop_speed_kmh_ / 3.6; - reason = "min_gap_below_stop_distance"; - } else if (min_gap_m < warning_distance_m_) { - target_speed_mps = warning_speed_kmh_ / 3.6; - reason = "min_gap_below_warning_distance"; + if (!std::isfinite(min_gap_m)) { + min_gap_m = -1.0; } - if (target_speed_mps < 0.0) { - return; + // Global ROS signal keeps the strictest fleet state. + std::string fleet_target_speed_mps_value = "nochange"; + std::string fleet_reason = "no_limit"; + if (min_gap_m >= 0.0 && min_gap_m < stop_distance_m_) { + fleet_target_speed_mps_value = std::to_string(stop_speed_kmh_ / 3.6); + fleet_reason = "min_gap_below_stop_distance"; + } else if (min_gap_m >= 0.0 && min_gap_m < warning_distance_m_) { + fleet_target_speed_mps_value = std::to_string(warning_speed_kmh_ / 3.6); + fleet_reason = "min_gap_below_warning_distance"; } - if (std::fabs(last_published_speed_mps_ - target_speed_mps) < 1e-6) { - return; - } - - // Keep ROS command as a global signal while TCP commands are individualized per truck. std_msgs::msg::String command; - command.data = "target_speed_mps=" + std::to_string(target_speed_mps) + - ";scope=all_connected_with_valid_tcp_and_fresh_cot" + ";reason=" + reason + + command.data = "target_speed_mps=" + fleet_target_speed_mps_value + + ";scope=all_connected_with_valid_tcp_and_fresh_cot" + ";reason=" + fleet_reason + ";min_gap_m=" + std::to_string(min_gap_m) + ";connected_count=" + std::to_string(connected.size()); @@ -782,19 +776,30 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { const std::string ahead_uid = has_ahead ? connected[i + 1].id : "none"; const double ahead_gap = has_ahead ? (connected[i + 1].distance_m - connected[i].distance_m) : -1.0; const int ahead_path_index = has_ahead ? connected[i + 1].path_index : -1; + const bool inhibit_start = has_ahead && ahead_gap < stop_distance_m_; + + std::string target_speed_mps_value = "nochange"; + std::string reason = "no_limit"; + if (inhibit_start) { + target_speed_mps_value = std::to_string(stop_speed_kmh_ / 3.6); + reason = "truck_ahead_below_stop_distance"; + } else if (has_ahead && ahead_gap < warning_distance_m_) { + target_speed_mps_value = std::to_string(warning_speed_kmh_ / 3.6); + reason = "truck_ahead_below_warning_distance"; + } const std::string tcp_command = - "target_speed_mps=" + std::to_string(target_speed_mps) + ";distance_to_truck_ahead_m=" + + "target_speed_mps=" + target_speed_mps_value + ";distance_to_truck_ahead_m=" + std::to_string(ahead_gap) + ";truck_ahead_path_index=" + std::to_string(ahead_path_index) + - ";truck_ahead_uid=" + ahead_uid + ";reason=" + reason + ";min_gap_m=" + std::to_string(min_gap_m) + + ";truck_ahead_uid=" + ahead_uid + ";inhibit_start=" + (inhibit_start ? "1" : "0") + + ";reason=" + reason + ";min_gap_m=" + std::to_string(min_gap_m) + ";connected_count=" + std::to_string(connected.size()); sendSpeedCommandToTcpClient(connected[i].id, tcp_command); } - last_published_speed_mps_ = target_speed_mps; RCLCPP_WARN(get_logger(), - "Published speed command %.2f m/s (reason=%s, min_gap=%.2f m, connected=%zu, first_id=%s)", - target_speed_mps, reason.c_str(), min_gap_m, connected.size(), + "Published fleet speed command target_speed_mps=%s (reason=%s, min_gap=%.2f m, connected=%zu, first_id=%s)", + fleet_target_speed_mps_value.c_str(), fleet_reason.c_str(), min_gap_m, connected.size(), connected.empty() ? "-" : connected.front().id.c_str()); } diff --git a/atos_gui/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js index d6feac4b1..b2d81815b 100644 --- a/atos_gui/atos_gui/static/fleet_map.js +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -259,7 +259,7 @@ const color = item.tcp_connected ? "#dc2626" : "#6b7280"; const ahead = aheadDistanceMap[uid]; const aheadText = - speedKmh > 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 194f4f367df4880e569966dedecea465dc17571c Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Fri, 17 Apr 2026 15:17:00 +0200 Subject: [PATCH 13/29] according to algorithm --- .../inc/truckobjectcontrol.hpp | 1 + .../src/truckobjectcontrol.cpp | 116 ++++++++++++------ 2 files changed, 80 insertions(+), 37 deletions(-) diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index e1bddc8cf..f0b23730d 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -37,6 +37,7 @@ class TruckObjectControl : public rclcpp::Node { bool tcp_connected = false; std::string path_name = ""; int path_index = -1; + std::string last_control_command = ""; rclcpp::Time last_cot_stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); }; diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index f4e8e3b11..639ff6564 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -722,8 +722,10 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ void TruckObjectControl::evaluateAndPublishSpeedCommand() { struct ConnectedTruck { std::string id; + std::string path_name; double distance_m = 0.0; int path_index = -1; + std::string previous_command; }; std::vector connected; @@ -734,7 +736,8 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { if (!state.tcp_connected || !isCotFresh(state, now_time)) { continue; } - connected.push_back(ConnectedTruck{id, state.distance_along_trajectory_m, state.path_index}); + connected.push_back(ConnectedTruck{id, state.path_name, state.distance_along_trajectory_m, + state.path_index, state.last_control_command}); } } @@ -742,24 +745,90 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { return; } - std::sort(connected.begin(), connected.end(), - [](const ConnectedTruck &a, const ConnectedTruck &b) { return a.distance_m < b.distance_m; }); + std::unordered_map> by_path; + for (const auto &truck : connected) { + by_path[truck.path_name].push_back(truck); + } double min_gap_m = std::numeric_limits::infinity(); - for (size_t i = 1; i < connected.size(); ++i) { - min_gap_m = std::min(min_gap_m, std::fabs(connected[i].distance_m - connected[i - 1].distance_m)); + bool any_stop = false; + bool any_slowdown = false; + size_t sent_commands = 0; + + for (auto &[path_name, group] : by_path) { + std::sort(group.begin(), group.end(), + [](const ConnectedTruck &a, const ConnectedTruck &b) { return a.distance_m < b.distance_m; }); + + for (size_t i = 0; i < group.size(); ++i) { + const bool has_ahead = (i + 1) < group.size(); + const std::string ahead_uid = has_ahead ? group[i + 1].id : "none"; + const double ahead_gap = has_ahead ? (group[i + 1].distance_m - group[i].distance_m) : -1.0; + const int ahead_path_index = has_ahead ? group[i + 1].path_index : -1; + + if (has_ahead) { + min_gap_m = std::min(min_gap_m, ahead_gap); + } + + const std::string previous_command = group[i].previous_command; + std::string control_command = previous_command.empty() ? "RESUME" : previous_command; + + const bool in_warning_band = has_ahead && (ahead_gap <= 500.0 && ahead_gap > 200.0); + const bool in_stop_band = has_ahead && (ahead_gap <= 200.0); + if (in_warning_band) { + control_command = "SLOWDOWN"; + } + if (in_stop_band) { + control_command = "STOP"; + } + if (has_ahead && ahead_gap > 300.0 && previous_command == "STOP") { + control_command = "SLOWDOWN"; + } + if (has_ahead && ahead_gap > 700.0 && previous_command == "SLOWDOWN") { + control_command = "RESUME"; + } + + std::string target_speed_mps_value = "nochange"; + if (control_command == "STOP") { + target_speed_mps_value = std::to_string(stop_speed_kmh_ / 3.6); + any_stop = true; + } else if (control_command == "SLOWDOWN") { + target_speed_mps_value = std::to_string(warning_speed_kmh_ / 3.6); + any_slowdown = true; + } + + { + std::lock_guard lock(state_mutex_); + auto it = trucks_.find(group[i].id); + if (it != trucks_.end()) { + it->second.last_control_command = control_command; + } + } + + const std::string reason = + (control_command == "STOP") + ? "truck_ahead_below_stop_distance" + : (control_command == "SLOWDOWN" ? "truck_ahead_slowdown_state" : "truck_ahead_resume_state"); + const std::string tcp_command = + "command=" + control_command + ";target_speed_mps=" + target_speed_mps_value + + ";distance_to_truck_ahead_m=" + std::to_string(ahead_gap) + + ";truck_ahead_path_index=" + std::to_string(ahead_path_index) + ";truck_ahead_uid=" + ahead_uid + + ";reason=" + reason + ";min_gap_m=" + (std::isfinite(min_gap_m) ? std::to_string(min_gap_m) : "-1") + + ";connected_count=" + std::to_string(connected.size()) + ";path_name=" + path_name; + sendSpeedCommandToTcpClient(group[i].id, tcp_command); + sent_commands += 1; + } } + if (!std::isfinite(min_gap_m)) { min_gap_m = -1.0; } - // Global ROS signal keeps the strictest fleet state. std::string fleet_target_speed_mps_value = "nochange"; std::string fleet_reason = "no_limit"; - if (min_gap_m >= 0.0 && min_gap_m < stop_distance_m_) { + if (any_stop) { fleet_target_speed_mps_value = std::to_string(stop_speed_kmh_ / 3.6); fleet_reason = "min_gap_below_stop_distance"; - } else if (min_gap_m >= 0.0 && min_gap_m < warning_distance_m_) { + } else if (any_slowdown) { fleet_target_speed_mps_value = std::to_string(warning_speed_kmh_ / 3.6); fleet_reason = "min_gap_below_warning_distance"; } @@ -769,38 +838,11 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { ";scope=all_connected_with_valid_tcp_and_fresh_cot" + ";reason=" + fleet_reason + ";min_gap_m=" + std::to_string(min_gap_m) + ";connected_count=" + std::to_string(connected.size()); - speed_command_pub_->publish(command); - for (size_t i = 0; i < connected.size(); ++i) { - const bool has_ahead = (i + 1) < connected.size(); - const std::string ahead_uid = has_ahead ? connected[i + 1].id : "none"; - const double ahead_gap = has_ahead ? (connected[i + 1].distance_m - connected[i].distance_m) : -1.0; - const int ahead_path_index = has_ahead ? connected[i + 1].path_index : -1; - const bool inhibit_start = has_ahead && ahead_gap < stop_distance_m_; - - std::string target_speed_mps_value = "nochange"; - std::string reason = "no_limit"; - if (inhibit_start) { - target_speed_mps_value = std::to_string(stop_speed_kmh_ / 3.6); - reason = "truck_ahead_below_stop_distance"; - } else if (has_ahead && ahead_gap < warning_distance_m_) { - target_speed_mps_value = std::to_string(warning_speed_kmh_ / 3.6); - reason = "truck_ahead_below_warning_distance"; - } - - const std::string tcp_command = - "target_speed_mps=" + target_speed_mps_value + ";distance_to_truck_ahead_m=" + - std::to_string(ahead_gap) + ";truck_ahead_path_index=" + std::to_string(ahead_path_index) + - ";truck_ahead_uid=" + ahead_uid + ";inhibit_start=" + (inhibit_start ? "1" : "0") + - ";reason=" + reason + ";min_gap_m=" + std::to_string(min_gap_m) + - ";connected_count=" + std::to_string(connected.size()); - sendSpeedCommandToTcpClient(connected[i].id, tcp_command); - } RCLCPP_WARN(get_logger(), - "Published fleet speed command target_speed_mps=%s (reason=%s, min_gap=%.2f m, connected=%zu, first_id=%s)", - fleet_target_speed_mps_value.c_str(), fleet_reason.c_str(), min_gap_m, connected.size(), - connected.empty() ? "-" : connected.front().id.c_str()); + "Published fleet speed command target_speed_mps=%s (reason=%s, min_gap=%.2f m, connected=%zu, tcp_cmds=%zu)", + fleet_target_speed_mps_value.c_str(), fleet_reason.c_str(), min_gap_m, connected.size(), sent_commands); } void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command) { From a6fe271a3166c59df110ddc934a7148e26c17adc Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Fri, 17 Apr 2026 22:51:00 +0200 Subject: [PATCH 14/29] calc distance when standing still --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 20494 -> 21159 bytes .../inc/truckobjectcontrol.hpp | 1 + .../src/truckobjectcontrol.cpp | 17 +++++++++++++++++ atos_gui/atos_gui/static/fleet_map.js | 11 ++++++----- 4 files changed, 24 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/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index f0b23730d..041c13387 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -38,6 +38,7 @@ class TruckObjectControl : public rclcpp::Node { std::string path_name = ""; int path_index = -1; std::string last_control_command = ""; + std::string last_tcp_command = ""; rclcpp::Time last_cot_stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); }; diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 639ff6564..632ed58bf 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -142,6 +142,7 @@ void TruckObjectControl::publishTruckState(const std::string &truck_id, const Tr payload["tcp_connected"] = state.tcp_connected; payload["path_name"] = state.path_name; payload["path_index"] = state.path_index; + payload["last_tcp_command"] = state.last_tcp_command; payload["stamp_sec"] = now().seconds(); std_msgs::msg::String msg; @@ -865,5 +866,21 @@ void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_i RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, "Failed to send TCP speed command to uid=%s fd=%d (errno=%d)", target_id.c_str(), target_fd, errno); + return; + } + + TruckState state_copy; + bool has_state = false; + { + std::lock_guard lock(state_mutex_); + const auto it = trucks_.find(target_id); + if (it != trucks_.end()) { + it->second.last_tcp_command = command; + state_copy = it->second; + has_state = true; + } + } + if (has_state) { + publishTruckState(target_id, state_copy); } } 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 9b0936e41f6cfa135a7f51c73189506482a2d960 Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Mon, 20 Apr 2026 15:41:28 +0200 Subject: [PATCH 15/29] TLS support --- atos/launch/launch_atosfleetmanagement.py | 18 +++ .../modules/TruckObjectControl/CMakeLists.txt | 3 + .../inc/truckobjectcontrol.hpp | 8 + atos/modules/TruckObjectControl/src/main.cpp | 2 + .../src/truckobjectcontrol.cpp | 145 ++++++++++++++++-- docker-compose-fleetmanagement.yml | 4 + scripts/atosfleetmanagement.env.example | 4 + scripts/installation/dependencies.txt | 3 +- scripts/installation/install_atos.sh | 8 +- scripts/run_atosfleetmanagement.sh | 29 +++- 10 files changed, 203 insertions(+), 21 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/atos/modules/TruckObjectControl/CMakeLists.txt b/atos/modules/TruckObjectControl/CMakeLists.txt index 10636a7a6..1da34fbbc 100644 --- a/atos/modules/TruckObjectControl/CMakeLists.txt +++ b/atos/modules/TruckObjectControl/CMakeLists.txt @@ -13,6 +13,7 @@ project(truck_object_control) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(OpenSSL REQUIRED) include(GNUInstallDirs) @@ -29,6 +30,8 @@ target_include_directories(${PROJECT_NAME} PUBLIC ) target_link_libraries(${PROJECT_NAME} ${COREUTILS_LIBRARY} + OpenSSL::SSL + OpenSSL::Crypto ) ament_target_dependencies(${PROJECT_NAME} diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index 041c13387..e075cb68c 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -10,6 +10,7 @@ #include #include +#include #include #include #include @@ -72,6 +73,10 @@ class TruckObjectControl : public rclcpp::Node { double cot_timeout_seconds_ = 2.0; int cot_tcp_port_ = 8114; std::string cot_tcp_bind_address_ = "0.0.0.0"; + bool cot_tls_require_client_cert_ = false; + std::string cot_tls_cert_path_ = ""; + std::string cot_tls_key_path_ = ""; + std::string cot_tls_ca_path_ = ""; std::string trajectory_geojson_path_ = ""; std::string default_path_name_ = ""; @@ -83,6 +88,8 @@ class TruckObjectControl : public rclcpp::Node { std::set tcp_client_fds_; std::mutex tcp_command_mutex_; std::unordered_map uid_to_client_fd_; + std::unordered_map uid_to_ssl_; + SSL_CTX *ssl_ctx_ = nullptr; void onCotMessage(const std_msgs::msg::String::SharedPtr msg); void evaluateAndPublishSpeedCommand(); @@ -103,4 +110,5 @@ class TruckObjectControl : public rclcpp::Node { void acceptTcpClients(); void handleTcpClient(int client_fd, const std::string &peer_name); void sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command); + bool initializeTlsContext(); }; diff --git a/atos/modules/TruckObjectControl/src/main.cpp b/atos/modules/TruckObjectControl/src/main.cpp index a8d56d02f..a0053b563 100644 --- a/atos/modules/TruckObjectControl/src/main.cpp +++ b/atos/modules/TruckObjectControl/src/main.cpp @@ -4,8 +4,10 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ #include "truckobjectcontrol.hpp" +#include int main(int argc, char **argv) { + std::signal(SIGPIPE, SIG_IGN); rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::spin(node); diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 632ed58bf..aa62270ed 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -14,6 +14,8 @@ #include #include +#include + #include #include #include @@ -25,6 +27,7 @@ #include #include #include +#include #include using std::placeholders::_1; @@ -88,6 +91,10 @@ TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { declare_parameter("cot_timeout_seconds", cot_timeout_seconds_); declare_parameter("cot_tcp_port", cot_tcp_port_); declare_parameter("cot_tcp_bind_address", cot_tcp_bind_address_); + declare_parameter("cot_tls_require_client_cert", cot_tls_require_client_cert_); + declare_parameter("cot_tls_cert_path", cot_tls_cert_path_); + declare_parameter("cot_tls_key_path", cot_tls_key_path_); + declare_parameter("cot_tls_ca_path", cot_tls_ca_path_); declare_parameter("trajectory_geojson_path", trajectory_geojson_path_); warning_distance_m_ = get_parameter("warning_distance_m").as_double(); @@ -97,6 +104,10 @@ TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { cot_timeout_seconds_ = get_parameter("cot_timeout_seconds").as_double(); cot_tcp_port_ = get_parameter("cot_tcp_port").as_int(); cot_tcp_bind_address_ = get_parameter("cot_tcp_bind_address").as_string(); + cot_tls_require_client_cert_ = get_parameter("cot_tls_require_client_cert").as_bool(); + cot_tls_cert_path_ = get_parameter("cot_tls_cert_path").as_string(); + cot_tls_key_path_ = get_parameter("cot_tls_key_path").as_string(); + cot_tls_ca_path_ = get_parameter("cot_tls_ca_path").as_string(); trajectory_geojson_path_ = get_parameter("trajectory_geojson_path").as_string(); trajectory_geojson_path_ = resolveDefaultTrajectoryPath(trajectory_geojson_path_); default_path_name_ = std::filesystem::path(trajectory_geojson_path_).filename().string(); @@ -122,12 +133,16 @@ TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { startTcpServer(); RCLCPP_INFO(get_logger(), - "TruckObjectControl started. Listening for COT XML on tcp://%s:%d and placeholder topic 'truck_objects/cot'.", + "TruckObjectControl started. Listening for COT XML on tls://%s:%d and placeholder topic 'truck_objects/cot'.", cot_tcp_bind_address_.c_str(), cot_tcp_port_); } TruckObjectControl::~TruckObjectControl() { stopTcpServer(); + if (ssl_ctx_ != nullptr) { + SSL_CTX_free(ssl_ctx_); + ssl_ctx_ = nullptr; + } } void TruckObjectControl::publishTruckState(const std::string &truck_id, const TruckState &state) { @@ -493,7 +508,72 @@ double TruckObjectControl::projectDistanceAlongTrajectory(double lat, double lon return best_distance_along_m; } +bool TruckObjectControl::initializeTlsContext() { + if (ssl_ctx_ != nullptr) { + return true; + } + + if (cot_tls_cert_path_.empty() || cot_tls_key_path_.empty()) { + RCLCPP_ERROR(get_logger(), "TLS enabled but cot_tls_cert_path or cot_tls_key_path is empty."); + return false; + } + + SSL_load_error_strings(); + OpenSSL_add_ssl_algorithms(); + + ssl_ctx_ = SSL_CTX_new(TLS_server_method()); + if (ssl_ctx_ == nullptr) { + RCLCPP_ERROR(get_logger(), "Failed to create TLS server context."); + return false; + } + + SSL_CTX_set_min_proto_version(ssl_ctx_, TLS1_2_VERSION); + + if (SSL_CTX_use_certificate_file(ssl_ctx_, cot_tls_cert_path_.c_str(), SSL_FILETYPE_PEM) != 1) { + RCLCPP_ERROR(get_logger(), "Failed to load TLS certificate from '%s'.", cot_tls_cert_path_.c_str()); + SSL_CTX_free(ssl_ctx_); + ssl_ctx_ = nullptr; + return false; + } + + if (SSL_CTX_use_PrivateKey_file(ssl_ctx_, cot_tls_key_path_.c_str(), SSL_FILETYPE_PEM) != 1) { + RCLCPP_ERROR(get_logger(), "Failed to load TLS private key from '%s'.", cot_tls_key_path_.c_str()); + SSL_CTX_free(ssl_ctx_); + ssl_ctx_ = nullptr; + return false; + } + + if (SSL_CTX_check_private_key(ssl_ctx_) != 1) { + RCLCPP_ERROR(get_logger(), "TLS private key does not match certificate."); + SSL_CTX_free(ssl_ctx_); + ssl_ctx_ = nullptr; + return false; + } + + if (!cot_tls_ca_path_.empty()) { + if (SSL_CTX_load_verify_locations(ssl_ctx_, cot_tls_ca_path_.c_str(), nullptr) != 1) { + RCLCPP_ERROR(get_logger(), "Failed to load TLS CA file from '%s'.", cot_tls_ca_path_.c_str()); + SSL_CTX_free(ssl_ctx_); + ssl_ctx_ = nullptr; + return false; + } + } + + if (cot_tls_require_client_cert_) { + SSL_CTX_set_verify(ssl_ctx_, SSL_VERIFY_PEER | SSL_VERIFY_FAIL_IF_NO_PEER_CERT, nullptr); + } else { + SSL_CTX_set_verify(ssl_ctx_, SSL_VERIFY_NONE, nullptr); + } + + return true; +} + void TruckObjectControl::startTcpServer() { + if (!initializeTlsContext()) { + RCLCPP_ERROR(get_logger(), "COT listener startup failed: TLS setup failed."); + return; + } + tcp_server_fd_ = ::socket(AF_INET, SOCK_STREAM, 0); if (tcp_server_fd_ < 0) { RCLCPP_ERROR(get_logger(), "Failed to create TCP socket for COT listener."); @@ -558,6 +638,7 @@ void TruckObjectControl::stopTcpServer() { { std::lock_guard lock(tcp_command_mutex_); uid_to_client_fd_.clear(); + uid_to_ssl_.clear(); } std::lock_guard lock(tcp_threads_mutex_); @@ -610,21 +691,44 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ std::string buffer; buffer.reserve(8 * 1024); std::set seen_uids; + SSL *ssl = nullptr; try { + ssl = SSL_new(ssl_ctx_); + if (ssl == nullptr) { + RCLCPP_WARN(get_logger(), "Failed to create TLS session for %s.", peer_name.c_str()); + throw std::runtime_error("SSL_new failed"); + } + + if (SSL_set_fd(ssl, client_fd) != 1) { + RCLCPP_WARN(get_logger(), "Failed to bind TLS session to socket for %s.", peer_name.c_str()); + throw std::runtime_error("SSL_set_fd failed"); + } + + if (SSL_accept(ssl) != 1) { + char ssl_error[256] = {0}; + ERR_error_string_n(ERR_get_error(), ssl_error, sizeof(ssl_error)); + RCLCPP_WARN(get_logger(), + "TLS handshake failed for %s: %s", + peer_name.c_str(), + ssl_error[0] == '\0' ? "unknown error" : ssl_error); + throw std::runtime_error("SSL_accept failed"); + } + char receive_buffer[kReceiveBufferSize]; while (tcp_running_.load()) { - const ssize_t received = ::recv(client_fd, receive_buffer, sizeof(receive_buffer), 0); - if (received == 0) { - break; - } - if (received < 0) { - if (errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK) { + const ssize_t received = SSL_read(ssl, receive_buffer, static_cast(sizeof(receive_buffer))); + if (received <= 0) { + const int ssl_read_error = SSL_get_error(ssl, static_cast(received)); + if (ssl_read_error == SSL_ERROR_WANT_READ || ssl_read_error == SSL_ERROR_WANT_WRITE) { continue; } + if (ssl_read_error == SSL_ERROR_ZERO_RETURN) { + break; + } RCLCPP_WARN(get_logger(), - "TCP receive error from %s (errno=%d). Closing this connection only.", - peer_name.c_str(), errno); + "TLS receive error from %s (ssl_error=%d). Closing this connection only.", + peer_name.c_str(), ssl_read_error); break; } @@ -675,6 +779,7 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ { std::lock_guard lock(tcp_command_mutex_); uid_to_client_fd_[observation.truck_id] = client_fd; + uid_to_ssl_[observation.truck_id] = ssl; } seen_uids.insert(observation.truck_id); @@ -708,6 +813,7 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ if (it != uid_to_client_fd_.end() && it->second == client_fd) { uid_to_client_fd_.erase(it); } + uid_to_ssl_.erase(uid); } } @@ -715,6 +821,11 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ std::lock_guard lock(tcp_threads_mutex_); tcp_client_fds_.erase(client_fd); } + if (ssl != nullptr) { + (void)SSL_shutdown(ssl); + SSL_free(ssl); + ssl = nullptr; + } ::shutdown(client_fd, SHUT_RDWR); ::close(client_fd); RCLCPP_INFO(get_logger(), "TruckObject TCP client disconnected: %s", peer_name.c_str()); @@ -848,24 +959,30 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command) { int target_fd = -1; + SSL *target_ssl = nullptr; { std::lock_guard lock(tcp_command_mutex_); const auto it = uid_to_client_fd_.find(target_id); if (it != uid_to_client_fd_.end()) { target_fd = it->second; } + const auto ssl_it = uid_to_ssl_.find(target_id); + if (ssl_it != uid_to_ssl_.end()) { + target_ssl = ssl_it->second; + } } - if (target_fd < 0) { + if (target_fd < 0 || target_ssl == nullptr) { return; } const std::string payload = command + "\n"; - const ssize_t sent = ::send(target_fd, payload.data(), payload.size(), MSG_NOSIGNAL); - if (sent < 0) { + const ssize_t sent = SSL_write(target_ssl, payload.data(), static_cast(payload.size())); + if (sent <= 0) { + const int ssl_write_error = SSL_get_error(target_ssl, static_cast(sent)); RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, - "Failed to send TCP speed command to uid=%s fd=%d (errno=%d)", target_id.c_str(), - target_fd, errno); + "Failed to send TLS speed command to uid=%s fd=%d (ssl_error=%d)", target_id.c_str(), + target_fd, ssl_write_error); return; } 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/dependencies.txt b/scripts/installation/dependencies.txt index e7c28e487..15765b353 100644 --- a/scripts/installation/dependencies.txt +++ b/scripts/installation/dependencies.txt @@ -13,4 +13,5 @@ python3-pip proj-bin clang - libstdc++-12-dev \ No newline at end of file + libstdc++-12-dev + libssl-dev 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 993543ba1c549e24b1cf4580b24c9a2b64ccf0af Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 20 Apr 2026 16:11:52 +0200 Subject: [PATCH 16/29] tcp mode activated when TLS mode is activated in TruckObjectControl when cot_tls_cert_path:=/path/to/server.crt or cot_tls_key_path:=/path/to/server.key is missing --- .../inc/truckobjectcontrol.hpp | 1 + .../src/truckobjectcontrol.cpp | 117 ++++++++++++------ 2 files changed, 79 insertions(+), 39 deletions(-) diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index e075cb68c..3baad9929 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -77,6 +77,7 @@ class TruckObjectControl : public rclcpp::Node { std::string cot_tls_cert_path_ = ""; std::string cot_tls_key_path_ = ""; std::string cot_tls_ca_path_ = ""; + bool cot_tls_enabled_ = false; std::string trajectory_geojson_path_ = ""; std::string default_path_name_ = ""; diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index aa62270ed..20a1d5bd2 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -108,6 +108,11 @@ TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { cot_tls_cert_path_ = get_parameter("cot_tls_cert_path").as_string(); cot_tls_key_path_ = get_parameter("cot_tls_key_path").as_string(); cot_tls_ca_path_ = get_parameter("cot_tls_ca_path").as_string(); + cot_tls_enabled_ = (!cot_tls_cert_path_.empty() && !cot_tls_key_path_.empty()); + if (!cot_tls_enabled_ && (!cot_tls_cert_path_.empty() || !cot_tls_key_path_.empty())) { + RCLCPP_WARN(get_logger(), + "Incomplete TLS config for COT listener (cert or key missing). Falling back to plain TCP."); + } trajectory_geojson_path_ = get_parameter("trajectory_geojson_path").as_string(); trajectory_geojson_path_ = resolveDefaultTrajectoryPath(trajectory_geojson_path_); default_path_name_ = std::filesystem::path(trajectory_geojson_path_).filename().string(); @@ -133,8 +138,8 @@ TruckObjectControl::TruckObjectControl() : Node("truck_object_control") { startTcpServer(); RCLCPP_INFO(get_logger(), - "TruckObjectControl started. Listening for COT XML on tls://%s:%d and placeholder topic 'truck_objects/cot'.", - cot_tcp_bind_address_.c_str(), cot_tcp_port_); + "TruckObjectControl started. Listening for COT XML on %s://%s:%d and placeholder topic 'truck_objects/cot'.", + cot_tls_enabled_ ? "tls" : "tcp", cot_tcp_bind_address_.c_str(), cot_tcp_port_); } TruckObjectControl::~TruckObjectControl() { @@ -569,7 +574,7 @@ bool TruckObjectControl::initializeTlsContext() { } void TruckObjectControl::startTcpServer() { - if (!initializeTlsContext()) { + if (cot_tls_enabled_ && !initializeTlsContext()) { RCLCPP_ERROR(get_logger(), "COT listener startup failed: TLS setup failed."); return; } @@ -694,42 +699,61 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ SSL *ssl = nullptr; try { - ssl = SSL_new(ssl_ctx_); - if (ssl == nullptr) { - RCLCPP_WARN(get_logger(), "Failed to create TLS session for %s.", peer_name.c_str()); - throw std::runtime_error("SSL_new failed"); - } + if (cot_tls_enabled_) { + ssl = SSL_new(ssl_ctx_); + if (ssl == nullptr) { + RCLCPP_WARN(get_logger(), "Failed to create TLS session for %s.", peer_name.c_str()); + throw std::runtime_error("SSL_new failed"); + } - if (SSL_set_fd(ssl, client_fd) != 1) { - RCLCPP_WARN(get_logger(), "Failed to bind TLS session to socket for %s.", peer_name.c_str()); - throw std::runtime_error("SSL_set_fd failed"); - } + if (SSL_set_fd(ssl, client_fd) != 1) { + RCLCPP_WARN(get_logger(), "Failed to bind TLS session to socket for %s.", peer_name.c_str()); + throw std::runtime_error("SSL_set_fd failed"); + } - if (SSL_accept(ssl) != 1) { - char ssl_error[256] = {0}; - ERR_error_string_n(ERR_get_error(), ssl_error, sizeof(ssl_error)); - RCLCPP_WARN(get_logger(), - "TLS handshake failed for %s: %s", - peer_name.c_str(), - ssl_error[0] == '\0' ? "unknown error" : ssl_error); - throw std::runtime_error("SSL_accept failed"); + if (SSL_accept(ssl) != 1) { + char ssl_error[256] = {0}; + ERR_error_string_n(ERR_get_error(), ssl_error, sizeof(ssl_error)); + RCLCPP_WARN(get_logger(), + "TLS handshake failed for %s: %s", + peer_name.c_str(), + ssl_error[0] == '\0' ? "unknown error" : ssl_error); + throw std::runtime_error("SSL_accept failed"); + } } char receive_buffer[kReceiveBufferSize]; while (tcp_running_.load()) { - const ssize_t received = SSL_read(ssl, receive_buffer, static_cast(sizeof(receive_buffer))); - if (received <= 0) { - const int ssl_read_error = SSL_get_error(ssl, static_cast(received)); - if (ssl_read_error == SSL_ERROR_WANT_READ || ssl_read_error == SSL_ERROR_WANT_WRITE) { - continue; + ssize_t received = 0; + if (cot_tls_enabled_) { + received = SSL_read(ssl, receive_buffer, static_cast(sizeof(receive_buffer))); + if (received <= 0) { + const int ssl_read_error = SSL_get_error(ssl, static_cast(received)); + if (ssl_read_error == SSL_ERROR_WANT_READ || ssl_read_error == SSL_ERROR_WANT_WRITE) { + continue; + } + if (ssl_read_error == SSL_ERROR_ZERO_RETURN) { + break; + } + RCLCPP_WARN(get_logger(), + "TLS receive error from %s (ssl_error=%d). Closing this connection only.", + peer_name.c_str(), ssl_read_error); + break; } - if (ssl_read_error == SSL_ERROR_ZERO_RETURN) { + } else { + received = ::recv(client_fd, receive_buffer, sizeof(receive_buffer), 0); + if (received == 0) { + break; + } + if (received < 0) { + if (errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK) { + continue; + } + RCLCPP_WARN(get_logger(), + "TCP receive error from %s (errno=%d). Closing this connection only.", + peer_name.c_str(), errno); break; } - RCLCPP_WARN(get_logger(), - "TLS receive error from %s (ssl_error=%d). Closing this connection only.", - peer_name.c_str(), ssl_read_error); - break; } buffer.append(receive_buffer, static_cast(received)); @@ -779,7 +803,9 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ { std::lock_guard lock(tcp_command_mutex_); uid_to_client_fd_[observation.truck_id] = client_fd; - uid_to_ssl_[observation.truck_id] = ssl; + if (cot_tls_enabled_) { + uid_to_ssl_[observation.truck_id] = ssl; + } } seen_uids.insert(observation.truck_id); @@ -972,18 +998,31 @@ void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_i } } - if (target_fd < 0 || target_ssl == nullptr) { + if (target_fd < 0) { return; } const std::string payload = command + "\n"; - const ssize_t sent = SSL_write(target_ssl, payload.data(), static_cast(payload.size())); - if (sent <= 0) { - const int ssl_write_error = SSL_get_error(target_ssl, static_cast(sent)); - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, - "Failed to send TLS speed command to uid=%s fd=%d (ssl_error=%d)", target_id.c_str(), - target_fd, ssl_write_error); - return; + if (cot_tls_enabled_) { + if (target_ssl == nullptr) { + return; + } + const ssize_t sent = SSL_write(target_ssl, payload.data(), static_cast(payload.size())); + if (sent <= 0) { + const int ssl_write_error = SSL_get_error(target_ssl, static_cast(sent)); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, + "Failed to send TLS speed command to uid=%s fd=%d (ssl_error=%d)", target_id.c_str(), + target_fd, ssl_write_error); + return; + } + } else { + const ssize_t sent = ::send(target_fd, payload.data(), payload.size(), MSG_NOSIGNAL); + if (sent < 0) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, + "Failed to send TCP speed command to uid=%s fd=%d (errno=%d)", target_id.c_str(), + target_fd, errno); + return; + } } TruckState state_copy; From d69b792a5e17d5b81a47b846bee2c700b3f00e08 Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Mon, 20 Apr 2026 16:11:23 +0200 Subject: [PATCH 17/29] Fix speed We assumed COT was mps but it was km/h --- atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 20a1d5bd2..fb9777765 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -283,7 +283,8 @@ bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation out.course_deg = 0.0; if (std::regex_search(payload, m, track_re) && m.size() >= 3) { try { - out.speed_mps = std::stod(m[1].str()); + // Incoming CoT track speed from PathFollower is in km/h; convert to m/s for ATOS internals. + out.speed_mps = std::stod(m[1].str()) / 3.6; out.course_deg = std::stod(m[2].str()); } catch (...) { out.speed_mps = 0.0; From 18894b14d74a1c6bb80360f19f5d438e78016c88 Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Wed, 22 Apr 2026 08:51:29 +0200 Subject: [PATCH 18/29] Path index calculated instead of sent in --- .../inc/truckobjectcontrol.hpp | 3 +- .../src/truckobjectcontrol.cpp | 92 +++++++++++++++---- atos_gui/atos_gui/static/fleet_map.js | 10 +- 3 files changed, 83 insertions(+), 22 deletions(-) diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index 3baad9929..4f44a6a20 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -104,7 +104,8 @@ class TruckObjectControl : public rclcpp::Node { std::string resolveTrajectoryPathByName(const std::string &path_name) const; const std::vector *getTrajectoryForPath(const std::string &path_name); double projectDistanceAlongTrajectory(double lat, double lon, - const std::vector *trajectory = nullptr) const; + const std::vector *trajectory = nullptr, + int *projected_path_index = nullptr) const; void startTcpServer(); void stopTcpServer(); diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index fb9777765..1c9303294 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -53,6 +53,22 @@ double geodesicDistanceMeters(double lat1, double lon1, double lat2, double lon2 return earth_radius_m * c; } +struct LocalXY { + double x = 0.0; + double y = 0.0; +}; + +LocalXY toLocalXY(double ref_lat_deg, double ref_lon_deg, double lat_deg, double lon_deg) { + constexpr double earth_radius_m = 6378137.0; + const double ref_lat_rad = degToRad(ref_lat_deg); + const double dlat = degToRad(lat_deg - ref_lat_deg); + const double dlon = degToRad(lon_deg - ref_lon_deg); + LocalXY out; + out.x = earth_radius_m * dlon * std::cos(ref_lat_rad); + out.y = earth_radius_m * dlat; + return out; +} + std::string resolveDefaultTrajectoryPath(const std::string &configured_path) { namespace fs = std::filesystem; if (!configured_path.empty() && fs::exists(configured_path)) { @@ -257,7 +273,6 @@ bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation static const std::regex point_re(R"re(]*\blat="([^"]+)"[^>]*\blon="([^"]+)")re"); static const std::regex track_re(R"re(]*\bspeed="([^"]+)"[^>]*\bcourse="([^"]+)")re"); static const std::regex path_name_re(R"re(]*\bpath_name="([^"]+)")re"); - static const std::regex path_index_re(R"re(]*\bpath_index="([^"]+)")re"); std::smatch m; if (!std::regex_search(payload, m, uid_re) || m.size() < 2) { @@ -297,13 +312,6 @@ bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation if (std::regex_search(payload, m, path_name_re) && m.size() >= 2) { out.path_name = m[1].str(); } - if (std::regex_search(payload, m, path_index_re) && m.size() >= 2) { - try { - out.path_index = std::stoi(m[1].str()); - } catch (...) { - out.path_index = -1; - } - } const std::vector *trajectory = nullptr; if (!out.path_name.empty()) { @@ -316,11 +324,10 @@ bool TruckObjectControl::parseCotXml(const std::string &payload, CotObservation } } - if (trajectory && out.path_index >= 0 && static_cast(out.path_index) < trajectory->size()) { - out.distance_along_trajectory_m = (*trajectory)[static_cast(out.path_index)].distance_m; - } else { - out.distance_along_trajectory_m = projectDistanceAlongTrajectory(out.lat, out.lon, trajectory); - } + int projected_path_index = -1; + out.distance_along_trajectory_m = + projectDistanceAlongTrajectory(out.lat, out.lon, trajectory, &projected_path_index); + out.path_index = projected_path_index; out.tcp_connected = true; return true; @@ -493,24 +500,58 @@ const std::vector *TruckObjectControl::getTrajecto } double TruckObjectControl::projectDistanceAlongTrajectory(double lat, double lon, - const std::vector *trajectory) const { + const std::vector *trajectory, + int *projected_path_index) const { const std::vector *path = trajectory; if (!path || path->empty()) { path = &trajectory_path_; } if (!path || path->empty()) { + if (projected_path_index != nullptr) { + *projected_path_index = -1; + } return 0.0; } + // Project onto each polyline segment to get a continuous along-track distance. + // This avoids quantizing multiple trucks to the same nearest vertex. double best_distance_to_path_m = std::numeric_limits::infinity(); double best_distance_along_m = 0.0; - for (const auto &p : *path) { - const double d = geodesicDistanceMeters(lat, lon, p.lat, p.lon); + int best_index = -1; + + for (size_t i = 0; i + 1 < path->size(); ++i) { + const auto &a = (*path)[i]; + const auto &b = (*path)[i + 1]; + const double segment_length_m = std::max(1e-6, b.distance_m - a.distance_m); + + const LocalXY a_xy{0.0, 0.0}; + const LocalXY b_xy = toLocalXY(a.lat, a.lon, b.lat, b.lon); + const LocalXY q_xy = toLocalXY(a.lat, a.lon, lat, lon); + + const double vx = b_xy.x - a_xy.x; + const double vy = b_xy.y - a_xy.y; + const double wx = q_xy.x - a_xy.x; + const double wy = q_xy.y - a_xy.y; + const double denom = vx * vx + vy * vy; + double t = 0.0; + if (denom > 1e-12) { + t = std::clamp((wx * vx + wy * vy) / denom, 0.0, 1.0); + } + + const double proj_lat = a.lat + (b.lat - a.lat) * t; + const double proj_lon = a.lon + (b.lon - a.lon) * t; + const double d = geodesicDistanceMeters(lat, lon, proj_lat, proj_lon); + if (d < best_distance_to_path_m) { best_distance_to_path_m = d; - best_distance_along_m = p.distance_m; + best_distance_along_m = a.distance_m + t * segment_length_m; + best_index = static_cast(t < 0.5 ? i : (i + 1)); } } + + if (projected_path_index != nullptr) { + *projected_path_index = best_index; + } return best_distance_along_m; } @@ -953,6 +994,23 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { ";truck_ahead_path_index=" + std::to_string(ahead_path_index) + ";truck_ahead_uid=" + ahead_uid + ";reason=" + reason + ";min_gap_m=" + (std::isfinite(min_gap_m) ? std::to_string(min_gap_m) : "-1") + ";connected_count=" + std::to_string(connected.size()) + ";path_name=" + path_name; + + // Keep UI state in sync with latest generated command, even if TCP/TLS send fails. + TruckState state_copy; + bool has_state = false; + { + std::lock_guard lock(state_mutex_); + auto it = trucks_.find(group[i].id); + if (it != trucks_.end()) { + it->second.last_tcp_command = tcp_command; + state_copy = it->second; + has_state = true; + } + } + if (has_state) { + publishTruckState(group[i].id, state_copy); + } + sendSpeedCommandToTcpClient(group[i].id, tcp_command); sent_commands += 1; } 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 3d5788856ccf29864729909e91b729ab3c9eb2cb Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Wed, 22 Apr 2026 15:43:02 +0200 Subject: [PATCH 19/29] Distances and wraparound works, but still some TCP message issue --- .../inc/truckobjectcontrol.hpp | 2 + .../src/truckobjectcontrol.cpp | 110 ++++++++++++++++-- atos_gui/atos_gui/main.py | 3 +- atos_gui/atos_gui/static/fleet_map.js | 12 +- 4 files changed, 109 insertions(+), 18 deletions(-) diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index 4f44a6a20..1f1b9d8b5 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -16,6 +16,7 @@ #include #include #include +#include class TruckObjectControl : public rclcpp::Node { public: @@ -91,6 +92,7 @@ class TruckObjectControl : public rclcpp::Node { std::unordered_map uid_to_client_fd_; std::unordered_map uid_to_ssl_; SSL_CTX *ssl_ctx_ = nullptr; + uint64_t tcp_command_seq_ = 0; void onCotMessage(const std_msgs::msg::String::SharedPtr msg); void evaluateAndPublishSpeedCommand(); diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 1c9303294..53648756c 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -844,6 +845,13 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ } { std::lock_guard lock(tcp_command_mutex_); + const auto existing = uid_to_client_fd_.find(observation.truck_id); + if (existing != uid_to_client_fd_.end() && existing->second != client_fd) { + RCLCPP_WARN(get_logger(), + "UID '%s' already mapped to fd=%d; remapping to fd=%d from %s. " + "Ensure each sender uses a unique uid.", + observation.truck_id.c_str(), existing->second, client_fd, peer_name.c_str()); + } uid_to_client_fd_[observation.truck_id] = client_fd; if (cot_tls_enabled_) { uid_to_ssl_[observation.truck_id] = ssl; @@ -904,24 +912,31 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { std::string id; std::string path_name; double distance_m = 0.0; + double speed_mps = 0.0; int path_index = -1; std::string previous_command; }; std::vector connected; - const auto now_time = now(); { std::lock_guard lock(state_mutex_); for (const auto &[id, state] : trucks_) { - if (!state.tcp_connected || !isCotFresh(state, now_time)) { + if (!state.tcp_connected) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 3000, + "Skipping uid=%s from command eval: tcp_connected=false, path=%s, distance=%.1fm", + id.c_str(), + state.path_name.c_str(), state.distance_along_trajectory_m); continue; } - connected.push_back(ConnectedTruck{id, state.path_name, state.distance_along_trajectory_m, + connected.push_back(ConnectedTruck{id, state.path_name, state.distance_along_trajectory_m, state.speed_mps, state.path_index, state.last_control_command}); } } if (connected.empty()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, + "No TCP-connected trucks available for command evaluation."); return; } @@ -930,6 +945,21 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { by_path[truck.path_name].push_back(truck); } + { + std::ostringstream summary; + summary << "Connected trucks=" << connected.size() << " ["; + for (size_t i = 0; i < connected.size(); ++i) { + const auto &t = connected[i]; + if (i > 0) { + summary << " | "; + } + summary << "uid=" << t.id << ",path=" << t.path_name << ",d=" << std::fixed << std::setprecision(1) + << t.distance_m; + } + summary << "]"; + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 3000, "%s", summary.str().c_str()); + } + double min_gap_m = std::numeric_limits::infinity(); bool any_stop = false; bool any_slowdown = false; @@ -939,11 +969,59 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { std::sort(group.begin(), group.end(), [](const ConnectedTruck &a, const ConnectedTruck &b) { return a.distance_m < b.distance_m; }); + double path_length_m = 0.0; + const std::vector *trajectory = nullptr; + if (!path_name.empty()) { + trajectory = getTrajectoryForPath(path_name); + } + if ((!trajectory || trajectory->empty()) && !trajectory_path_.empty()) { + trajectory = &trajectory_path_; + } + if (trajectory && !trajectory->empty()) { + path_length_m = trajectory->back().distance_m; + } + for (size_t i = 0; i < group.size(); ++i) { - const bool has_ahead = (i + 1) < group.size(); - const std::string ahead_uid = has_ahead ? group[i + 1].id : "none"; - const double ahead_gap = has_ahead ? (group[i + 1].distance_m - group[i].distance_m) : -1.0; - const int ahead_path_index = has_ahead ? group[i + 1].path_index : -1; + const bool has_peers = group.size() > 1; + bool has_ahead = has_peers; + std::string ahead_uid = "none"; + double ahead_gap = -1.0; + int ahead_path_index = -1; + + if (has_peers) { + const size_t ahead_index = (i + 1) % group.size(); + ahead_uid = group[ahead_index].id; + ahead_path_index = group[ahead_index].path_index; + const int curr_path_index = group[i].path_index; + + // Prefer index-based circular gap when both indices are valid for this trajectory. + bool used_index_gap = false; + if (trajectory && !trajectory->empty() && curr_path_index >= 0 && ahead_path_index >= 0 && + static_cast(curr_path_index) < trajectory->size() && + static_cast(ahead_path_index) < trajectory->size() && path_length_m > 0.0) { + const double curr_d = (*trajectory)[static_cast(curr_path_index)].distance_m; + const double ahead_d = (*trajectory)[static_cast(ahead_path_index)].distance_m; + ahead_gap = ahead_d - curr_d; + if (ahead_gap <= 0.0) { + // Wraparound: distance to end + distance from beginning. + ahead_gap += path_length_m; + } + used_index_gap = true; + } + + if (!used_index_gap) { + ahead_gap = group[ahead_index].distance_m - group[i].distance_m; + if (ahead_gap <= 0.0 && path_length_m > 0.0) { + ahead_gap += path_length_m; + } + } + if (ahead_gap <= 0.0) { + has_ahead = false; + ahead_uid = "none"; + ahead_path_index = -1; + ahead_gap = -1.0; + } + } if (has_ahead) { min_gap_m = std::min(min_gap_m, ahead_gap); @@ -967,7 +1045,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { control_command = "RESUME"; } - std::string target_speed_mps_value = "nochange"; + std::string target_speed_mps_value = std::to_string(std::max(0.0, group[i].speed_mps)); if (control_command == "STOP") { target_speed_mps_value = std::to_string(stop_speed_kmh_ / 3.6); any_stop = true; @@ -988,12 +1066,20 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { (control_command == "STOP") ? "truck_ahead_below_stop_distance" : (control_command == "SLOWDOWN" ? "truck_ahead_slowdown_state" : "truck_ahead_resume_state"); + uint64_t cmd_seq = 0; + { + std::lock_guard lock(state_mutex_); + cmd_seq = ++tcp_command_seq_; + } const std::string tcp_command = "command=" + control_command + ";target_speed_mps=" + target_speed_mps_value + ";distance_to_truck_ahead_m=" + std::to_string(ahead_gap) + ";truck_ahead_path_index=" + std::to_string(ahead_path_index) + ";truck_ahead_uid=" + ahead_uid + ";reason=" + reason + ";min_gap_m=" + (std::isfinite(min_gap_m) ? std::to_string(min_gap_m) : "-1") + - ";connected_count=" + std::to_string(connected.size()) + ";path_name=" + path_name; + ";connected_count=" + std::to_string(connected.size()) + ";path_name=" + path_name + + ";cmd_seq=" + std::to_string(cmd_seq); + + RCLCPP_INFO(get_logger(), "Prepared TCP command for uid=%s: %s", group[i].id.c_str(), tcp_command.c_str()); // Keep UI state in sync with latest generated command, even if TCP/TLS send fails. TruckState state_copy; @@ -1032,7 +1118,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { std_msgs::msg::String command; command.data = "target_speed_mps=" + fleet_target_speed_mps_value + - ";scope=all_connected_with_valid_tcp_and_fresh_cot" + ";reason=" + fleet_reason + + ";scope=all_connected_with_valid_tcp" + ";reason=" + fleet_reason + ";min_gap_m=" + std::to_string(min_gap_m) + ";connected_count=" + std::to_string(connected.size()); speed_command_pub_->publish(command); @@ -1074,6 +1160,8 @@ void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_i target_fd, ssl_write_error); return; } + RCLCPP_INFO(get_logger(), "Sent TLS speed command to uid=%s fd=%d: %s", target_id.c_str(), target_fd, + command.c_str()); } else { const ssize_t sent = ::send(target_fd, payload.data(), payload.size(), MSG_NOSIGNAL); if (sent < 0) { @@ -1082,6 +1170,8 @@ void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_i target_fd, errno); return; } + RCLCPP_INFO(get_logger(), "Sent TCP speed command to uid=%s fd=%d: %s", target_id.c_str(), target_fd, + command.c_str()); } TruckState state_copy; diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index 784a82644..e4c67c50a 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] = {} @@ -152,7 +153,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 be4f4e2d4207996be5d61fe376d72c4e52915d6b Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Thu, 23 Apr 2026 16:06:53 +0200 Subject: [PATCH 20/29] Tcp client can reconnect. Report tcp error in UI. COT and TCP displayed in UI. --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 21159 -> 3943 bytes .../inc/truckobjectcontrol.hpp | 2 + .../src/atostrucksimulator.cpp | 4 +- .../src/truckobjectcontrol.cpp | 48 ++++++++++++------ atos_gui/atos_gui/main.py | 2 +- atos_gui/atos_gui/static/fleet_map.js | 36 ++++++++++--- 6 files changed, 68 insertions(+), 24 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/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index 1f1b9d8b5..0188f8629 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -41,6 +41,8 @@ class TruckObjectControl : public rclcpp::Node { int path_index = -1; std::string last_control_command = ""; std::string last_tcp_command = ""; + std::string last_tcp_warning = ""; + std::string last_cot_message = ""; rclcpp::Time last_cot_stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); }; diff --git a/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp index ed825c51d..8fb71a74d 100644 --- a/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp +++ b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp @@ -342,11 +342,11 @@ void AtosTruckSimulator::simulationStep() { cot << "<__group name=\"Dark Green\" role=\"Test Vehicle\"/>"; const std::string payload = cot.str(); diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 53648756c..0447d8f73 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -179,7 +179,9 @@ void TruckObjectControl::publishTruckState(const std::string &truck_id, const Tr payload["tcp_connected"] = state.tcp_connected; payload["path_name"] = state.path_name; payload["path_index"] = state.path_index; + payload["last_cot_message"] = state.last_cot_message; payload["last_tcp_command"] = state.last_tcp_command; + payload["last_tcp_warning"] = state.last_tcp_warning; payload["stamp_sec"] = now().seconds(); std_msgs::msg::String msg; @@ -207,6 +209,8 @@ void TruckObjectControl::onCotMessage(const std_msgs::msg::String::SharedPtr msg entry.tcp_connected = observation.tcp_connected; entry.path_name = observation.path_name; entry.path_index = observation.path_index; + entry.last_cot_message = msg->data; + entry.last_tcp_warning = ""; entry.last_cot_stamp = now(); state = entry; } @@ -840,6 +844,8 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ entry.tcp_connected = true; entry.path_name = observation.path_name; entry.path_index = observation.path_index; + entry.last_cot_message = xml; + entry.last_tcp_warning = ""; entry.last_cot_stamp = now(); state = entry; } @@ -875,6 +881,7 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ const auto it = trucks_.find(uid); if (it != trucks_.end()) { it->second.tcp_connected = false; + it->second.last_tcp_warning = "TCP client disconnected"; it->second.last_cot_stamp = now(); state = it->second; found = true; @@ -1129,6 +1136,24 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { } void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command) { + auto update_tcp_status = [&](const std::string &warning) { + TruckState state_copy; + bool has_state = false; + { + std::lock_guard lock(state_mutex_); + const auto it = trucks_.find(target_id); + if (it != trucks_.end()) { + it->second.last_tcp_command = command; + it->second.last_tcp_warning = warning; + state_copy = it->second; + has_state = true; + } + } + if (has_state) { + publishTruckState(target_id, state_copy); + } + }; + int target_fd = -1; SSL *target_ssl = nullptr; { @@ -1144,12 +1169,14 @@ void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_i } if (target_fd < 0) { + update_tcp_status("No active TCP socket for this truck"); return; } const std::string payload = command + "\n"; if (cot_tls_enabled_) { if (target_ssl == nullptr) { + update_tcp_status("TLS enabled but no SSL session bound to this truck"); return; } const ssize_t sent = SSL_write(target_ssl, payload.data(), static_cast(payload.size())); @@ -1158,6 +1185,7 @@ void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_i RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, "Failed to send TLS speed command to uid=%s fd=%d (ssl_error=%d)", target_id.c_str(), target_fd, ssl_write_error); + update_tcp_status("TLS send failed (ssl_error=" + std::to_string(ssl_write_error) + ")"); return; } RCLCPP_INFO(get_logger(), "Sent TLS speed command to uid=%s fd=%d: %s", target_id.c_str(), target_fd, @@ -1165,27 +1193,17 @@ void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_i } else { const ssize_t sent = ::send(target_fd, payload.data(), payload.size(), MSG_NOSIGNAL); if (sent < 0) { + const int send_errno = errno; RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, "Failed to send TCP speed command to uid=%s fd=%d (errno=%d)", target_id.c_str(), - target_fd, errno); + target_fd, send_errno); + update_tcp_status("TCP send failed (errno=" + std::to_string(send_errno) + ")"); return; } RCLCPP_INFO(get_logger(), "Sent TCP speed command to uid=%s fd=%d: %s", target_id.c_str(), target_fd, command.c_str()); } - TruckState state_copy; - bool has_state = false; - { - std::lock_guard lock(state_mutex_); - const auto it = trucks_.find(target_id); - if (it != trucks_.end()) { - it->second.last_tcp_command = command; - state_copy = it->second; - has_state = true; - } - } - if (has_state) { - publishTruckState(target_id, state_copy); - } + // Clear previous warning on successful send. + update_tcp_status(""); } diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index e4c67c50a..f3d8f513f 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 e3bc7aa9b32548509c62ce889b1c5f46701bbbcf Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Fri, 24 Apr 2026 08:58:39 +0200 Subject: [PATCH 21/29] 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 f3d8f513f..964c84787 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 a1b79c9634543b61a89f1bfc590d5520eef4fa35 Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Fri, 24 Apr 2026 09:21:57 +0200 Subject: [PATCH 22/29] 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 69387b6bff30d6bba4972964c1d40a8229d135b9 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Fri, 24 Apr 2026 12:02:00 +0200 Subject: [PATCH 23/29] TCP commands are sent immediately --- atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 0447d8f73..01bd384ba 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -721,6 +722,12 @@ void TruckObjectControl::acceptTcpClients() { int keepalive = 1; (void)setsockopt(client_fd, SOL_SOCKET, SO_KEEPALIVE, &keepalive, sizeof(keepalive)); + int tcp_no_delay = 1; + if (::setsockopt(client_fd, IPPROTO_TCP, TCP_NODELAY, &tcp_no_delay, sizeof(tcp_no_delay)) < 0) { + RCLCPP_WARN(get_logger(), + "Failed to enable TCP_NODELAY for client socket (errno=%d). Commands may be delayed.", errno); + } + timeval recv_timeout{}; recv_timeout.tv_sec = 1; recv_timeout.tv_usec = 0; From cf948105f23ed626ad7d0a9f67198dd0a3c2b5b1 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 27 Apr 2026 08:56:37 +0200 Subject: [PATCH 24/29] 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 40286e4e410fc112ce199022812c0d096efb0806 Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Mon, 27 Apr 2026 10:25:58 +0200 Subject: [PATCH 25/29] TSL/TCP Clients are handeled independantly If one crashes ATOS can still send data to the other. --- .../inc/truckobjectcontrol.hpp | 22 ++ .../src/truckobjectcontrol.cpp | 347 +++++++++++++----- 2 files changed, 276 insertions(+), 93 deletions(-) diff --git a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp index 0188f8629..6283fae16 100644 --- a/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -9,6 +9,8 @@ #include #include +#include +#include #include #include #include @@ -58,6 +60,20 @@ class TruckObjectControl : public rclcpp::Node { int path_index = -1; }; + struct TcpClientSession { + int fd = -1; + std::string peer_name = ""; + SSL *ssl = nullptr; + std::mutex io_mutex; + std::mutex queue_mutex; + std::condition_variable queue_cv; + std::thread sender_thread; + std::string queued_uid = ""; + std::string queued_command = ""; + bool has_queued_command = false; + bool stop_sender = false; + }; + rclcpp::Subscription::SharedPtr cot_sub_; rclcpp::Publisher::SharedPtr speed_command_pub_; rclcpp::Publisher::SharedPtr truck_state_pub_; @@ -91,6 +107,8 @@ class TruckObjectControl : public rclcpp::Node { std::mutex tcp_threads_mutex_; std::set tcp_client_fds_; std::mutex tcp_command_mutex_; + std::mutex tcp_sessions_mutex_; + std::unordered_map> tcp_sessions_; std::unordered_map uid_to_client_fd_; std::unordered_map uid_to_ssl_; SSL_CTX *ssl_ctx_ = nullptr; @@ -115,6 +133,10 @@ class TruckObjectControl : public rclcpp::Node { void stopTcpServer(); void acceptTcpClients(); void handleTcpClient(int client_fd, const std::string &peer_name); + void clientSenderLoop(const std::shared_ptr &session); + void disconnectClientSession(const std::shared_ptr &session, const std::string &reason); + void updateTruckTcpStatus(const std::string &target_id, const std::string &command, const std::string &warning, + bool mark_disconnected = false); void sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command); bool initializeTlsContext(); }; diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 01bd384ba..a99e6152b 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -38,6 +39,7 @@ using json = nlohmann::json; namespace { constexpr size_t kReceiveBufferSize = 4096; constexpr int kAcceptPollSleepMs = 100; +constexpr int kTlsHandshakeRetrySleepMs = 10; constexpr const char *kDefaultGeoJsonName = "RuralRoad_center_of_driving_lane_ccw.geojson"; double degToRad(double value) { @@ -693,6 +695,16 @@ void TruckObjectControl::stopTcpServer() { uid_to_client_fd_.clear(); uid_to_ssl_.clear(); } + { + std::lock_guard lock(tcp_sessions_mutex_); + for (auto &[fd, session] : tcp_sessions_) { + { + std::lock_guard qlock(session->queue_mutex); + session->stop_sender = true; + } + session->queue_cv.notify_all(); + } + } std::lock_guard lock(tcp_threads_mutex_); for (auto &thread : tcp_client_threads_) { @@ -701,6 +713,15 @@ void TruckObjectControl::stopTcpServer() { } } tcp_client_threads_.clear(); + { + std::lock_guard session_lock(tcp_sessions_mutex_); + for (auto &[fd, session] : tcp_sessions_) { + if (session->sender_thread.joinable()) { + session->sender_thread.join(); + } + } + tcp_sessions_.clear(); + } } void TruckObjectControl::acceptTcpClients() { @@ -728,6 +749,11 @@ void TruckObjectControl::acceptTcpClients() { "Failed to enable TCP_NODELAY for client socket (errno=%d). Commands may be delayed.", errno); } + const int flags = ::fcntl(client_fd, F_GETFL, 0); + if (flags >= 0) { + (void)::fcntl(client_fd, F_SETFL, flags | O_NONBLOCK); + } + timeval recv_timeout{}; recv_timeout.tv_sec = 1; recv_timeout.tv_usec = 0; @@ -740,6 +766,14 @@ void TruckObjectControl::acceptTcpClients() { RCLCPP_INFO(get_logger(), "Accepted TruckObject TCP client %s", peer.str().c_str()); + { + std::lock_guard lock(tcp_sessions_mutex_); + auto session = std::make_shared(); + session->fd = client_fd; + session->peer_name = peer.str(); + tcp_sessions_[client_fd] = session; + } + std::lock_guard lock(tcp_threads_mutex_); tcp_client_fds_.insert(client_fd); tcp_client_threads_.emplace_back(&TruckObjectControl::handleTcpClient, this, client_fd, peer.str()); @@ -751,6 +785,21 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ buffer.reserve(8 * 1024); std::set seen_uids; SSL *ssl = nullptr; + std::shared_ptr session; + + { + std::lock_guard lock(tcp_sessions_mutex_); + const auto it = tcp_sessions_.find(client_fd); + if (it != tcp_sessions_.end()) { + session = it->second; + } + } + if (!session) { + RCLCPP_WARN(get_logger(), "Missing TCP client session for %s fd=%d", peer_name.c_str(), client_fd); + ::shutdown(client_fd, SHUT_RDWR); + ::close(client_fd); + return; + } try { if (cot_tls_enabled_) { @@ -765,22 +814,42 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ throw std::runtime_error("SSL_set_fd failed"); } - if (SSL_accept(ssl) != 1) { + SSL_set_mode(ssl, SSL_MODE_ENABLE_PARTIAL_WRITE | SSL_MODE_ACCEPT_MOVING_WRITE_BUFFER); + + while (tcp_running_.load()) { + const int accept_result = SSL_accept(ssl); + if (accept_result == 1) { + break; + } + + const int ssl_accept_error = SSL_get_error(ssl, accept_result); + if (ssl_accept_error == SSL_ERROR_WANT_READ || ssl_accept_error == SSL_ERROR_WANT_WRITE) { + std::this_thread::sleep_for(std::chrono::milliseconds(kTlsHandshakeRetrySleepMs)); + continue; + } + char ssl_error[256] = {0}; ERR_error_string_n(ERR_get_error(), ssl_error, sizeof(ssl_error)); RCLCPP_WARN(get_logger(), - "TLS handshake failed for %s: %s", + "TLS handshake failed for %s: %s (ssl_error=%d)", peer_name.c_str(), - ssl_error[0] == '\0' ? "unknown error" : ssl_error); + ssl_error[0] == '\0' ? "unknown error" : ssl_error, + ssl_accept_error); throw std::runtime_error("SSL_accept failed"); } } + session->ssl = ssl; + session->sender_thread = std::thread(&TruckObjectControl::clientSenderLoop, this, session); + char receive_buffer[kReceiveBufferSize]; while (tcp_running_.load()) { ssize_t received = 0; if (cot_tls_enabled_) { - received = SSL_read(ssl, receive_buffer, static_cast(sizeof(receive_buffer))); + { + std::lock_guard lock(session->io_mutex); + received = SSL_read(ssl, receive_buffer, static_cast(sizeof(receive_buffer))); + } if (received <= 0) { const int ssl_read_error = SSL_get_error(ssl, static_cast(received)); if (ssl_read_error == SSL_ERROR_WANT_READ || ssl_read_error == SSL_ERROR_WANT_WRITE) { @@ -880,45 +949,7 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ peer_name.c_str()); } - for (const auto &uid : seen_uids) { - TruckState state; - bool found = false; - { - std::lock_guard lock(state_mutex_); - const auto it = trucks_.find(uid); - if (it != trucks_.end()) { - it->second.tcp_connected = false; - it->second.last_tcp_warning = "TCP client disconnected"; - it->second.last_cot_stamp = now(); - state = it->second; - found = true; - } - } - if (found) { - publishTruckState(uid, state); - } - { - std::lock_guard lock(tcp_command_mutex_); - const auto it = uid_to_client_fd_.find(uid); - if (it != uid_to_client_fd_.end() && it->second == client_fd) { - uid_to_client_fd_.erase(it); - } - uid_to_ssl_.erase(uid); - } - } - - { - std::lock_guard lock(tcp_threads_mutex_); - tcp_client_fds_.erase(client_fd); - } - if (ssl != nullptr) { - (void)SSL_shutdown(ssl); - SSL_free(ssl); - ssl = nullptr; - } - ::shutdown(client_fd, SHUT_RDWR); - ::close(client_fd); - RCLCPP_INFO(get_logger(), "TruckObject TCP client disconnected: %s", peer_name.c_str()); + disconnectClientSession(session, "TCP client disconnected"); } void TruckObjectControl::evaluateAndPublishSpeedCommand() { @@ -1142,75 +1173,205 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { fleet_target_speed_mps_value.c_str(), fleet_reason.c_str(), min_gap_m, connected.size(), sent_commands); } -void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command) { - auto update_tcp_status = [&](const std::string &warning) { - TruckState state_copy; - bool has_state = false; +void TruckObjectControl::updateTruckTcpStatus(const std::string &target_id, const std::string &command, + const std::string &warning, bool mark_disconnected) { + TruckState state_copy; + bool has_state = false; + { + std::lock_guard lock(state_mutex_); + const auto it = trucks_.find(target_id); + if (it != trucks_.end()) { + it->second.last_tcp_command = command; + it->second.last_tcp_warning = warning; + if (mark_disconnected) { + it->second.tcp_connected = false; + it->second.last_cot_stamp = now(); + } + state_copy = it->second; + has_state = true; + } + } + if (has_state) { + publishTruckState(target_id, state_copy); + } +} + +void TruckObjectControl::clientSenderLoop(const std::shared_ptr &session) { + while (tcp_running_.load()) { + std::string target_id; + std::string command; { - std::lock_guard lock(state_mutex_); - const auto it = trucks_.find(target_id); - if (it != trucks_.end()) { - it->second.last_tcp_command = command; - it->second.last_tcp_warning = warning; - state_copy = it->second; - has_state = true; + std::unique_lock lock(session->queue_mutex); + session->queue_cv.wait(lock, [&]() { return session->stop_sender || session->has_queued_command; }); + if (session->stop_sender) { + break; } + target_id = session->queued_uid; + command = session->queued_command; + session->has_queued_command = false; } - if (has_state) { - publishTruckState(target_id, state_copy); + + if (target_id.empty() || command.empty()) { + continue; } - }; + const std::string payload = command + "\n"; + if (cot_tls_enabled_) { + SSL *ssl = session->ssl; + if (ssl == nullptr) { + updateTruckTcpStatus(target_id, command, "TLS session missing for this truck", true); + ::shutdown(session->fd, SHUT_RDWR); + break; + } + + int sent = 0; + { + std::lock_guard lock(session->io_mutex); + sent = SSL_write(ssl, payload.data(), static_cast(payload.size())); + } + + if (sent <= 0) { + const int ssl_write_error = SSL_get_error(ssl, sent); + if (ssl_write_error == SSL_ERROR_WANT_READ || ssl_write_error == SSL_ERROR_WANT_WRITE) { + updateTruckTcpStatus(target_id, command, "TLS backpressure drop"); + continue; + } + updateTruckTcpStatus(target_id, command, + "TLS send failed (ssl_error=" + std::to_string(ssl_write_error) + ")", true); + ::shutdown(session->fd, SHUT_RDWR); + break; + } + + if (static_cast(sent) < payload.size()) { + updateTruckTcpStatus(target_id, command, "TLS partial send drop"); + continue; + } + + RCLCPP_INFO(get_logger(), "Sent TLS speed command to uid=%s fd=%d: %s", target_id.c_str(), session->fd, + command.c_str()); + updateTruckTcpStatus(target_id, command, ""); + continue; + } + + const ssize_t sent = ::send(session->fd, payload.data(), payload.size(), MSG_NOSIGNAL | MSG_DONTWAIT); + if (sent < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) { + updateTruckTcpStatus(target_id, command, "TCP backpressure drop"); + continue; + } + updateTruckTcpStatus(target_id, command, "TCP send failed (errno=" + std::to_string(errno) + ")", true); + ::shutdown(session->fd, SHUT_RDWR); + break; + } + + if (static_cast(sent) < payload.size()) { + updateTruckTcpStatus(target_id, command, "TCP partial send drop"); + continue; + } + + RCLCPP_INFO(get_logger(), "Sent TCP speed command to uid=%s fd=%d: %s", target_id.c_str(), session->fd, + command.c_str()); + updateTruckTcpStatus(target_id, command, ""); + } +} + +void TruckObjectControl::disconnectClientSession(const std::shared_ptr &session, + const std::string &reason) { + if (!session) { + return; + } + + { + std::lock_guard lock(session->queue_mutex); + session->stop_sender = true; + session->has_queued_command = false; + } + session->queue_cv.notify_all(); + + std::vector affected_uids; + { + std::lock_guard lock(tcp_command_mutex_); + for (auto it = uid_to_client_fd_.begin(); it != uid_to_client_fd_.end();) { + if (it->second == session->fd) { + affected_uids.push_back(it->first); + uid_to_ssl_.erase(it->first); + it = uid_to_client_fd_.erase(it); + } else { + ++it; + } + } + } + + for (const auto &uid : affected_uids) { + updateTruckTcpStatus(uid, "", reason, true); + } + + const int old_fd = session->fd; + { + std::lock_guard lock(tcp_threads_mutex_); + tcp_client_fds_.erase(old_fd); + } + + if (session->ssl != nullptr) { + std::lock_guard lock(session->io_mutex); + (void)SSL_shutdown(session->ssl); + SSL_free(session->ssl); + session->ssl = nullptr; + } + + if (old_fd >= 0) { + ::shutdown(old_fd, SHUT_RDWR); + ::close(old_fd); + session->fd = -1; + } + + if (session->sender_thread.joinable()) { + session->sender_thread.join(); + } + + { + std::lock_guard lock(tcp_sessions_mutex_); + tcp_sessions_.erase(old_fd); + } + + RCLCPP_INFO(get_logger(), "TruckObject TCP client disconnected: %s (%s)", session->peer_name.c_str(), + reason.c_str()); +} + +void TruckObjectControl::sendSpeedCommandToTcpClient(const std::string &target_id, const std::string &command) { int target_fd = -1; - SSL *target_ssl = nullptr; { std::lock_guard lock(tcp_command_mutex_); const auto it = uid_to_client_fd_.find(target_id); if (it != uid_to_client_fd_.end()) { target_fd = it->second; } - const auto ssl_it = uid_to_ssl_.find(target_id); - if (ssl_it != uid_to_ssl_.end()) { - target_ssl = ssl_it->second; - } } if (target_fd < 0) { - update_tcp_status("No active TCP socket for this truck"); + updateTruckTcpStatus(target_id, command, "No active TCP socket for this truck"); return; } - const std::string payload = command + "\n"; - if (cot_tls_enabled_) { - if (target_ssl == nullptr) { - update_tcp_status("TLS enabled but no SSL session bound to this truck"); - return; - } - const ssize_t sent = SSL_write(target_ssl, payload.data(), static_cast(payload.size())); - if (sent <= 0) { - const int ssl_write_error = SSL_get_error(target_ssl, static_cast(sent)); - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, - "Failed to send TLS speed command to uid=%s fd=%d (ssl_error=%d)", target_id.c_str(), - target_fd, ssl_write_error); - update_tcp_status("TLS send failed (ssl_error=" + std::to_string(ssl_write_error) + ")"); - return; - } - RCLCPP_INFO(get_logger(), "Sent TLS speed command to uid=%s fd=%d: %s", target_id.c_str(), target_fd, - command.c_str()); - } else { - const ssize_t sent = ::send(target_fd, payload.data(), payload.size(), MSG_NOSIGNAL); - if (sent < 0) { - const int send_errno = errno; - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, - "Failed to send TCP speed command to uid=%s fd=%d (errno=%d)", target_id.c_str(), - target_fd, send_errno); - update_tcp_status("TCP send failed (errno=" + std::to_string(send_errno) + ")"); - return; + std::shared_ptr session; + { + std::lock_guard lock(tcp_sessions_mutex_); + const auto it = tcp_sessions_.find(target_fd); + if (it != tcp_sessions_.end()) { + session = it->second; } - RCLCPP_INFO(get_logger(), "Sent TCP speed command to uid=%s fd=%d: %s", target_id.c_str(), target_fd, - command.c_str()); } - // Clear previous warning on successful send. - update_tcp_status(""); + if (!session) { + updateTruckTcpStatus(target_id, command, "No active TCP session for this truck"); + return; + } + + { + std::lock_guard lock(session->queue_mutex); + session->queued_uid = target_id; + session->queued_command = command; + session->has_queued_command = true; + } + session->queue_cv.notify_one(); } From 77ad6a182a41d3a24b1132f5fad463357778c427 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Mon, 27 Apr 2026 11:07:15 +0200 Subject: [PATCH 26/29] DRIVE command replaces RESUME --- ROS2_ATOSFleetManagement_cheatsheet.pdf | Bin 3943 -> 5231 bytes TruckObjectControl_Illustration.pdf | Bin 14713 -> 14135 bytes .../src/truckobjectcontrol.cpp | 6 +++--- 3 files changed, 3 insertions(+), 3 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 diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index a99e6152b..d8b250c01 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -1073,7 +1073,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { } const std::string previous_command = group[i].previous_command; - std::string control_command = previous_command.empty() ? "RESUME" : previous_command; + std::string control_command = previous_command.empty() ? "DRIVE" : previous_command; const bool in_warning_band = has_ahead && (ahead_gap <= 500.0 && ahead_gap > 200.0); const bool in_stop_band = has_ahead && (ahead_gap <= 200.0); @@ -1087,7 +1087,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { control_command = "SLOWDOWN"; } if (has_ahead && ahead_gap > 700.0 && previous_command == "SLOWDOWN") { - control_command = "RESUME"; + control_command = "DRIVE"; } std::string target_speed_mps_value = std::to_string(std::max(0.0, group[i].speed_mps)); @@ -1110,7 +1110,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { const std::string reason = (control_command == "STOP") ? "truck_ahead_below_stop_distance" - : (control_command == "SLOWDOWN" ? "truck_ahead_slowdown_state" : "truck_ahead_resume_state"); + : (control_command == "SLOWDOWN" ? "truck_ahead_slowdown_state" : "truck_ahead_drive_state"); uint64_t cmd_seq = 0; { std::lock_guard lock(state_mutex_); From 8f8296c43f39b9ebc238415988ed6cd9ddaea2f2 Mon Sep 17 00:00:00 2001 From: Jonathan Ahlstedt Date: Mon, 27 Apr 2026 11:08:36 +0200 Subject: [PATCH 27/29] Fix TCP connection stability --- .../src/truckobjectcontrol.cpp | 130 +++++++++++++++--- 1 file changed, 108 insertions(+), 22 deletions(-) diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index d8b250c01..8829979c3 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -8,10 +8,12 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -40,6 +42,9 @@ namespace { constexpr size_t kReceiveBufferSize = 4096; constexpr int kAcceptPollSleepMs = 100; constexpr int kTlsHandshakeRetrySleepMs = 10; +constexpr int kTlsReadPollTimeoutMs = 1000; +constexpr int kTlsIoRetrySleepMs = 10; +constexpr int kTlsWriteMaxTransientRetries = 100; constexpr const char *kDefaultGeoJsonName = "RuralRoad_center_of_driving_lane_ccw.geojson"; double degToRad(double value) { @@ -73,6 +78,24 @@ LocalXY toLocalXY(double ref_lat_deg, double ref_lon_deg, double lat_deg, double return out; } +std::string describeTlsError(const int ssl_error) { + char ssl_error_text[256] = {0}; + const unsigned long queued_error = ERR_get_error(); + if (queued_error != 0) { + ERR_error_string_n(queued_error, ssl_error_text, sizeof(ssl_error_text)); + } + + std::ostringstream out; + out << "ssl_error=" << ssl_error; + if (ssl_error_text[0] != '\0') { + out << ", openssl='" << ssl_error_text << "'"; + } + if (errno != 0) { + out << ", errno=" << errno << " (" << std::strerror(errno) << ")"; + } + return out.str(); +} + std::string resolveDefaultTrajectoryPath(const std::string &configured_path) { namespace fs = std::filesystem; if (!configured_path.empty() && fs::exists(configured_path)) { @@ -846,6 +869,30 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ while (tcp_running_.load()) { ssize_t received = 0; if (cot_tls_enabled_) { + pollfd pfd{}; + pfd.fd = client_fd; + pfd.events = POLLIN; + const int poll_result = ::poll(&pfd, 1, kTlsReadPollTimeoutMs); + if (poll_result == 0) { + continue; + } + if (poll_result < 0) { + if (errno == EINTR) { + continue; + } + RCLCPP_WARN(get_logger(), + "TLS receive poll error from %s (errno=%d: %s). Closing this connection only.", + peer_name.c_str(), errno, std::strerror(errno)); + break; + } + if ((pfd.revents & (POLLERR | POLLHUP | POLLNVAL)) != 0) { + RCLCPP_INFO(get_logger(), "TLS client %s socket closed or errored (revents=0x%x).", + peer_name.c_str(), pfd.revents); + break; + } + + ERR_clear_error(); + errno = 0; { std::lock_guard lock(session->io_mutex); received = SSL_read(ssl, receive_buffer, static_cast(sizeof(receive_buffer))); @@ -853,14 +900,19 @@ void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_ if (received <= 0) { const int ssl_read_error = SSL_get_error(ssl, static_cast(received)); if (ssl_read_error == SSL_ERROR_WANT_READ || ssl_read_error == SSL_ERROR_WANT_WRITE) { + std::this_thread::sleep_for(std::chrono::milliseconds(kTlsIoRetrySleepMs)); continue; } if (ssl_read_error == SSL_ERROR_ZERO_RETURN) { break; } + if (ssl_read_error == SSL_ERROR_SYSCALL && + (errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK)) { + continue; + } RCLCPP_WARN(get_logger(), - "TLS receive error from %s (ssl_error=%d). Closing this connection only.", - peer_name.c_str(), ssl_read_error); + "TLS receive error from %s (%s). Closing this connection only.", + peer_name.c_str(), describeTlsError(ssl_read_error).c_str()); break; } } else { @@ -1217,34 +1269,68 @@ void TruckObjectControl::clientSenderLoop(const std::shared_ptrssl; - if (ssl == nullptr) { + if (session->ssl == nullptr) { updateTruckTcpStatus(target_id, command, "TLS session missing for this truck", true); ::shutdown(session->fd, SHUT_RDWR); break; } - int sent = 0; - { - std::lock_guard lock(session->io_mutex); - sent = SSL_write(ssl, payload.data(), static_cast(payload.size())); - } + size_t total_sent = 0; + int transient_retries = 0; + bool send_failed = false; + std::string send_warning; + while (total_sent < payload.size() && tcp_running_.load()) { + { + std::lock_guard lock(session->queue_mutex); + if (session->stop_sender) { + send_failed = true; + send_warning = "TLS sender stopped"; + break; + } + } + + int sent = 0; + ERR_clear_error(); + errno = 0; + { + std::lock_guard lock(session->io_mutex); + sent = SSL_write(session->ssl, + payload.data() + total_sent, + static_cast(payload.size() - total_sent)); + } + + if (sent > 0) { + total_sent += static_cast(sent); + transient_retries = 0; + continue; + } - if (sent <= 0) { - const int ssl_write_error = SSL_get_error(ssl, sent); - if (ssl_write_error == SSL_ERROR_WANT_READ || ssl_write_error == SSL_ERROR_WANT_WRITE) { - updateTruckTcpStatus(target_id, command, "TLS backpressure drop"); + const int ssl_write_error = SSL_get_error(session->ssl, sent); + const bool transient = + ssl_write_error == SSL_ERROR_WANT_READ || ssl_write_error == SSL_ERROR_WANT_WRITE || + (ssl_write_error == SSL_ERROR_SYSCALL && + (errno == EINTR || errno == EAGAIN || errno == EWOULDBLOCK)); + if (transient && transient_retries < kTlsWriteMaxTransientRetries) { + transient_retries += 1; + std::this_thread::sleep_for(std::chrono::milliseconds(kTlsIoRetrySleepMs)); continue; } - updateTruckTcpStatus(target_id, command, - "TLS send failed (ssl_error=" + std::to_string(ssl_write_error) + ")", true); + + send_failed = true; + send_warning = "TLS send failed (" + describeTlsError(ssl_write_error) + ")"; + break; + } + + if (!send_failed && total_sent < payload.size()) { + updateTruckTcpStatus(target_id, command, "TLS sender stopped before payload was fully sent", true); ::shutdown(session->fd, SHUT_RDWR); break; } - if (static_cast(sent) < payload.size()) { - updateTruckTcpStatus(target_id, command, "TLS partial send drop"); - continue; + if (send_failed) { + updateTruckTcpStatus(target_id, command, send_warning, true); + ::shutdown(session->fd, SHUT_RDWR); + break; } RCLCPP_INFO(get_logger(), "Sent TLS speed command to uid=%s fd=%d: %s", target_id.c_str(), session->fd, @@ -1312,6 +1398,10 @@ void TruckObjectControl::disconnectClientSession(const std::shared_ptrsender_thread.joinable()) { + session->sender_thread.join(); + } + if (session->ssl != nullptr) { std::lock_guard lock(session->io_mutex); (void)SSL_shutdown(session->ssl); @@ -1325,10 +1415,6 @@ void TruckObjectControl::disconnectClientSession(const std::shared_ptrfd = -1; } - if (session->sender_thread.joinable()) { - session->sender_thread.join(); - } - { std::lock_guard lock(tcp_sessions_mutex_); tcp_sessions_.erase(old_fd); From f3d416845cee4ff6cc27946e949939ea0adfbff7 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Mon, 11 May 2026 12:45:00 +0200 Subject: [PATCH 28/29] Build for 20, 22 and 24 --- README.md | 2 +- atos/CMakeLists.txt | 2 +- atos/common/module.hpp | 2 +- atos/common/roschannels/roschannel.hpp | 4 +- atos/common/util.c | 14 ++--- .../DirectControl/src/directcontrol.cpp | 2 +- .../EsminiAdapter/src/esminiadapter.cpp | 14 +++-- .../src/scenarioexecution.cpp | 12 ++-- atos/modules/MQTTBridge/src/mqttbridge.cpp | 21 ++++--- .../ObjectControl/inc/relativeanchor.hpp | 2 +- .../ObjectControl/inc/relativetestobject.hpp | 2 +- atos/modules/ObjectControl/inc/testobject.hpp | 2 +- .../ObjectControl/src/objectcontrol.cpp | 12 ++-- .../ObjectControl/src/objectlistener.cpp | 6 +- .../inc/trajectoryletstreamer.hpp | 4 +- docs/Installation/installation.md | 2 +- scripts/installation/install_atos.sh | 25 ++++++++ scripts/installation/install_deps.sh | 58 +++++++++++++++++-- scripts/installation/install_functions.sh | 56 +++++++++++++++++- setup_atos.sh | 38 +++++++----- 20 files changed, 213 insertions(+), 67 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 d8c4f0fe0..721b74c50 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 0a648ebba..ff62b2e27 100644 --- a/atos/common/module.hpp +++ b/atos/common/module.hpp @@ -45,7 +45,7 @@ Msg_T msgCtr1(MsgData_T data) { class Module : public rclcpp::Node { public: Module(const std::string name) : Node(name), getStatusResponsePub(*this) {}; - Module() = default; + Module() = delete; bool shouldExit(); protected: diff --git a/atos/common/roschannels/roschannel.hpp b/atos/common/roschannels/roschannel.hpp index afdbd7b57..8e608f4aa 100644 --- a/atos/common/roschannels/roschannel.hpp +++ b/atos/common/roschannels/roschannel.hpp @@ -19,6 +19,7 @@ class BasePub { 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); }; }; @@ -32,7 +33,8 @@ class BaseSub { 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 \ No newline at end of file +} // namespace ROSChannels diff --git a/atos/common/util.c b/atos/common/util.c index 07362761f..8d1672350 100644 --- a/atos/common/util.c +++ b/atos/common/util.c @@ -1328,7 +1328,7 @@ int UtilFindCurrentTrajectoryPositionNew(ObjectPosition * OP, int StartIndex, do if (i <= -1) i = 0; OP->BestFoundTrajectoryIndex = 0; - fprintf("UtilFindCurrentTrajectoryPositionNew: TrajectoryPositionCount=%d, SyncIndex=%d, OrigoDistance=%4.3f, x=%4.3f, y=%4.3f, SyncIndex=%d\n", + fprintf(stderr, "UtilFindCurrentTrajectoryPositionNew: TrajectoryPositionCount=%d, SyncIndex=%d, OrigoDistance=%4.3f, x=%4.3f, y=%4.3f, SyncIndex=%d\n", OP->TrajectoryPositionCount, OP->SyncIndex, OP->OrigoDistance, OP->x, OP->y, OP->SyncIndex); Init = 1; @@ -1377,8 +1377,8 @@ int UtilFindCurrentTrajectoryPositionNew(ObjectPosition * OP, int StartIndex, do PositionFound = i; //SampledSpaceIndex[j] = i; //j++ ; - if (debug == 2) - fprintf("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; } @@ -1753,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) = NULL; - 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 ea625ab6c..52f15125f 100644 --- a/atos/modules/DirectControl/src/directcontrol.cpp +++ b/atos/modules/DirectControl/src/directcontrol.cpp @@ -178,7 +178,7 @@ void DirectControl::readTCPSocketData() { 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); } } diff --git a/atos/modules/EsminiAdapter/src/esminiadapter.cpp b/atos/modules/EsminiAdapter/src/esminiadapter.cpp index 50851c1ca..72691b879 100644 --- a/atos/modules/EsminiAdapter/src/esminiadapter.cpp +++ b/atos/modules/EsminiAdapter/src/esminiadapter.cpp @@ -51,8 +51,10 @@ 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, rmw_qos_profile_services_default, oscFilePathClient_cb_group_); - objectIdsClient_ = create_client(ServiceNames::getObjectIds, rmw_qos_profile_services_default, objectIdsClient_cb_group_); + oscFilePathClient_ = create_client( + ServiceNames::getOpenScenarioFilePath, rclcpp::ServicesQoS(), oscFilePathClient_cb_group_); + objectIdsClient_ = create_client( + ServiceNames::getObjectIds, rclcpp::ServicesQoS(), objectIdsClient_cb_group_); declare_parameter("timestep", 0.1); } @@ -524,7 +526,7 @@ void EsminiAdapter::runEsminiSimulation() me->applyTrajTransform = true; } catch (std::exception& e) { - RCLCPP_ERROR(me->get_logger(), e.what()); + RCLCPP_ERROR(me->get_logger(), "%s", e.what()); return; } } else { @@ -538,7 +540,7 @@ void EsminiAdapter::runEsminiSimulation() std::promise idsFetched; auto objectNameAndAtosIDsCallback = [&](rclcpp::Client::SharedFutureWithRequest future) { auto response = future.get(); - for (int i = 0; i < response.second->ids.size(); i++) { + 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]; } @@ -563,7 +565,8 @@ void EsminiAdapter::runEsminiSimulation() 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()); + 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()); @@ -586,7 +589,6 @@ void EsminiAdapter::onRequestObjectTrajectory( const std::shared_ptr req, std::shared_ptr res) { - res->id; me->fetchOSCFilePath(); if (me->runSimulation || me->atosObjectIdToTraj.find(req->id) == me->atosObjectIdToTraj.end()) { me->runEsminiSimulation(); diff --git a/atos/modules/IntegrationTesting/src/scenarioexecution.cpp b/atos/modules/IntegrationTesting/src/scenarioexecution.cpp index cc8cc9334..7a62cd7a2 100644 --- a/atos/modules/IntegrationTesting/src/scenarioexecution.cpp +++ b/atos/modules/IntegrationTesting/src/scenarioexecution.cpp @@ -128,8 +128,9 @@ void ScenarioExecution::printResult() { 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 @@ -143,7 +144,8 @@ 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()); } @@ -152,4 +154,6 @@ void ScenarioExecution::printResult() { * * @param msg Message. */ -void ScenarioExecution::placeholderCallback(const atos_interfaces::msg::Monitor::SharedPtr msg) {} \ No newline at end of file +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 782a0a71a..5f109f168 100644 --- a/atos/modules/MQTTBridge/src/mqttbridge.cpp +++ b/atos/modules/MQTTBridge/src/mqttbridge.cpp @@ -101,11 +101,12 @@ void MqttBridge::loadParameters() { 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(), - fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter " - "'ros_topic', will be ignored", - mqtt_topic) - .c_str()); + "%s", warning.c_str()); } } @@ -137,11 +138,12 @@ void MqttBridge::loadParameters() { 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(), - fmt::format("Parameter 'ros2mqtt.{}' is missing subparameter " - "'mqtt_topic', will be ignored", - ros_topic) - .c_str()); + "%s", warning.c_str()); } } } @@ -255,6 +257,7 @@ void MqttBridge::setupSubscriptions() { 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; @@ -469,4 +472,4 @@ void MqttBridge::on_failure(const mqtt::token &token) { "retry...", token.get_return_code()); is_connected_ = false; -} \ No newline at end of file +} diff --git a/atos/modules/ObjectControl/inc/relativeanchor.hpp b/atos/modules/ObjectControl/inc/relativeanchor.hpp index 9b1d54a7d..885e5abeb 100644 --- a/atos/modules/ObjectControl/inc/relativeanchor.hpp +++ b/atos/modules/ObjectControl/inc/relativeanchor.hpp @@ -15,7 +15,7 @@ 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; diff --git a/atos/modules/ObjectControl/inc/relativetestobject.hpp b/atos/modules/ObjectControl/inc/relativetestobject.hpp index 2fd6a746f..72ada9bef 100644 --- a/atos/modules/ObjectControl/inc/relativetestobject.hpp +++ b/atos/modules/ObjectControl/inc/relativetestobject.hpp @@ -16,7 +16,7 @@ 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, diff --git a/atos/modules/ObjectControl/inc/testobject.hpp b/atos/modules/ObjectControl/inc/testobject.hpp index 6886ca3cd..5b57affec 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 9b3f01fce..0d1b44b71 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, 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( @@ -367,7 +367,7 @@ void ObjectControl::loadObjectFiles() { throw std::invalid_argument(errMsg); } } catch (std::invalid_argument& e) { - RCLCPP_ERROR(get_logger(), e.what()); + RCLCPP_ERROR(get_logger(), "%s", e.what()); errors.push_back(e); } } @@ -519,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}); } diff --git a/atos/modules/ObjectControl/src/objectlistener.cpp b/atos/modules/ObjectControl/src/objectlistener.cpp index 96e695d5d..e67a05cee 100644 --- a/atos/modules/ObjectControl/src/objectlistener.cpp +++ b/atos/modules/ObjectControl/src/objectlistener.cpp @@ -35,11 +35,11 @@ void ObjectListener::listen() { obj->handleISOMessage(true); } } catch (std::invalid_argument& e) { - RCLCPP_ERROR(get_logger(), e.what()); // TODO: add comment explaining this case.. + RCLCPP_ERROR(get_logger(), "%s", 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_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 c60da4c4e..f33b84feb 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_deps.sh b/scripts/installation/install_deps.sh index 79ba25a23..2bb464667 100755 --- a/scripts/installation/install_deps.sh +++ b/scripts/installation/install_deps.sh @@ -19,6 +19,33 @@ fi source "${ATOS_REPO_PATH}/scripts/installation/install_functions.sh" +ATOS_VENV_PATH="$(get_atos_venv_path)" + +ensure_repo_submodules() { + if ! command -v git >/dev/null 2>&1; then + return 0 + fi + + if ! git -C "${ATOS_REPO_PATH}" rev-parse --is-inside-work-tree >/dev/null 2>&1; then + return 0 + fi + + echo "Updating git submodules required for dependency resolution..." + git -C "${ATOS_REPO_PATH}" submodule update --init --recursive +} + +venv_needs_rebuild() { + if [ ! -f "${ATOS_VENV_PATH}/pyvenv.cfg" ]; then + return 0 + fi + + if ! grep -q "^include-system-site-packages = true$" "${ATOS_VENV_PATH}/pyvenv.cfg"; then + return 0 + fi + + return 1 +} + apt_update_retry() { local attempts=5 local delay=5 @@ -56,8 +83,20 @@ 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 -python3 -m pip install -r ${ATOS_REPO_PATH}/scripts/installation/requirements.txt +apt_install_retry python3-pip python3-venv + +if [ ! -d "${ATOS_VENV_PATH}" ] || [ "$REINSTALL" = true ] || venv_needs_rebuild; then + echo "Creating ATOS Python virtual environment at ${ATOS_VENV_PATH}..." + rm -rf "${ATOS_VENV_PATH}" + python3 -m venv --system-site-packages "${ATOS_VENV_PATH}" +fi + +# Activate a dedicated virtual environment so Ubuntu 24.04+ does not require +# installing Python packages into the externally managed system interpreter. +# shellcheck disable=SC1090 +source "${ATOS_VENV_PATH}/bin/activate" +python -m pip install --upgrade pip +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." @@ -65,7 +104,15 @@ check_command_failed $? "Failed to install dependencies." ####################################### ###### Install ROS2 dependencies ###### ####################################### -ROS_DISTRO=humble +ROS_DISTRO="${ROS_DISTRO:-$(get_supported_ros_distro)}" + +if [ -z "${ROS_DISTRO}" ]; then + echo "Failed to determine a supported ROS 2 distribution for this Ubuntu release." + exit 1 +fi + +ensure_repo_submodules +check_command_failed $? "Failed to initialize ATOS git submodules." # Check if the ROS2 repository is already added if ! (apt list | grep -q "ros-$ROS_DISTRO-desktop"); then @@ -73,7 +120,7 @@ if ! (apt list | grep -q "ros-$ROS_DISTRO-desktop"); then # Install ROS2 prerequisites apt_update_retry - apt_install_retry lsb-release ros-dev-tools + apt_install_retry lsb-release # Authorize the ROS2 gpg key with apt sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ @@ -89,6 +136,7 @@ fi echo "Installing ROS2 packages..." apt_update_retry apt_install_retry \ + ros-dev-tools \ ros-${ROS_DISTRO}-desktop \ python3-rosdep \ ros-${ROS_DISTRO}-launch-pytest @@ -103,7 +151,7 @@ check_command_failed $? "Failed to install ROS2 dependencies." ######## Install ATOS GUI dependencies ######## ############################################### -python3 -m pip install -r ${ATOS_REPO_PATH}/atos_gui/requirements.txt +python -m pip install -r "${ATOS_REPO_PATH}/atos_gui/requirements.txt" ########################################### ###### Install some deps from source ###### 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 8d2fceae2306233539a15842084df232039a761e Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Mon, 11 May 2026 13:09:00 +0200 Subject: [PATCH 29/29] 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(