diff --git a/ATOSFleetManagement_architecture_and_dev_guide.pdf b/ATOSFleetManagement_architecture_and_dev_guide.pdf new file mode 100644 index 000000000..2dfd6ee55 Binary files /dev/null and b/ATOSFleetManagement_architecture_and_dev_guide.pdf differ 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 c3c554b97..3c57d25c8 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. @@ -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 ``` @@ -46,6 +46,77 @@ 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/). +## 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_atosfleetmanagement.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 +``` + +### 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. @@ -73,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 new file mode 100644 index 000000000..317559e11 Binary files /dev/null and b/ROS2_ATOSFleetManagement_cheatsheet.pdf differ diff --git a/TruckObjectControl_Illustration.pdf b/TruckObjectControl_Illustration.pdf new file mode 100644 index 000000000..7e388796d Binary files /dev/null and b/TruckObjectControl_Illustration.pdf differ diff --git a/atos/CMakeLists.txt b/atos/CMakeLists.txt index 2d83dba86..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() @@ -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/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/launch/launch_atosfleetmanagement.py b/atos/launch/launch_atosfleetmanagement.py new file mode 100644 index 000000000..72bf26486 --- /dev/null +++ b/atos/launch/launch_atosfleetmanagement.py @@ -0,0 +1,177 @@ +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') + 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}, + {'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, + simulator_launch_arg, + cot_tls_require_client_cert_launch_arg, + cot_tls_cert_path_launch_arg, + cot_tls_key_path_launch_arg, + cot_tls_ca_path_launch_arg, + Node( + condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('insecure')])), + package='atos_gui', + namespace='atos', + executable='truck_object_gui', + name='truck_object_gui', + output='screen', + arguments=['True', 'atosfleetmanagement'], + ), + Node( + condition=IfCondition(LaunchConfiguration('insecure')), + package='atos_gui', + namespace='atos', + executable='truck_object_gui', + name='truck_object_gui', + output='screen', + arguments=['False', 'atosfleetmanagement'], + ), + Node( + package='atos', + namespace='atos', + executable='truck_object_control', + name='truck_object_control', + output='screen', + parameters=[ + {'cot_tls_require_client_cert': cot_tls_require_client_cert}, + {'cot_tls_cert_path': cot_tls_cert_path}, + {'cot_tls_key_path': cot_tls_key_path}, + {'cot_tls_ca_path': cot_tls_ca_path}, + ], + ), + Node( + condition=IfCondition(with_simulator), + package='atos', + namespace='atos', + executable='atos_truck_simulator', + name='atos_truck_simulator_1', + output='screen', + parameters=[ + {'uid': 'L5S-TRUCK-SIM-1'}, + {'start_index': 0}, + {'target_speed_kmh': 80.0}, + {'acceleration_mps2': 2.0}, + {'ignore_warning_speed_commands': True}, + ], + ), + Node( + condition=IfCondition(with_simulator), + package='atos', + namespace='atos', + executable='atos_truck_simulator', + name='atos_truck_simulator_2', + output='screen', + parameters=[ + {'uid': 'L5S-TRUCK-SIM-2'}, + {'start_index': 250}, + {'target_speed_kmh': 40.0}, + {'acceleration_mps2': 2.0}, + ], + ), + Node( + condition=IfCondition(with_simulator), + package='atos', + namespace='atos', + executable='atos_truck_simulator', + name='atos_truck_simulator_3', + output='screen', + parameters=[ + {'uid': 'L5S-TRUCK-SIM-3'}, + {'start_index': 500}, + {'target_speed_kmh': 40.0}, + {'acceleration_mps2': 2.0}, + ], + ), + Node( + condition=IfCondition(PythonExpression(['not ', foxbridge])), + name='rosapi', + package='rosapi', + executable='rosapi_node', + ), + Node( + condition=IfCondition(PythonExpression([insecure_websockets, ' and ', foxbridge])), + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + output={'both': 'log'}, + parameters=fox_bridge_params, + ), + Node( + condition=IfCondition(PythonExpression(['not ', insecure_websockets, ' and ', foxbridge])), + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + output={'both': 'log'}, + parameters=fox_tls_bridge_params, + ), + Node( + condition=IfCondition(PythonExpression([insecure_websockets, ' and not ', foxbridge])), + package='rosbridge_server', + executable='rosbridge_websocket', + name='ros_bridge', + output={'both': 'log'}, + parameters=ros_bridge_params, + ), + Node( + condition=IfCondition(PythonExpression(['not ', insecure_websockets, ' and not ', foxbridge])), + package='rosbridge_server', + executable='rosbridge_websocket', + name='ros_bridge', + output={'both': 'log'}, + parameters=ros_tls_bridge_params, + ), + ]) diff --git a/atos/modules/DirectControl/src/directcontrol.cpp b/atos/modules/DirectControl/src/directcontrol.cpp index 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 6b4da4917..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); + } +} +} + @@ -51,8 +66,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, rmw_qos_profile_services_default, oscFilePathClient_cb_group_); + objectIdsClient_ = create_client( + ServiceNames::getObjectIds, rmw_qos_profile_services_default, objectIdsClient_cb_group_); declare_parameter("timestep", 0.1); } @@ -230,8 +247,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); + reportObjectPos(&SE_ReportObjectPos, esminiObjectId, SE_GetSimulationTime(), pos.x, pos.y, pos.z, yaw, pitch, roll); SE_ReportObjectSpeed(esminiObjectId, speed.x); } @@ -525,7 +541,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 { @@ -539,7 +555,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]; } @@ -564,7 +580,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()); @@ -587,7 +604,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..35f761f8e 100644 --- a/atos/modules/ObjectControl/src/objectcontrol.cpp +++ b/atos/modules/ObjectControl/src/objectcontrol.cpp @@ -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/atos/modules/TruckObjectControl/CMakeLists.txt b/atos/modules/TruckObjectControl/CMakeLists.txt new file mode 100644 index 000000000..1da34fbbc --- /dev/null +++ b/atos/modules/TruckObjectControl/CMakeLists.txt @@ -0,0 +1,73 @@ +# 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) +find_package(ament_index_cpp REQUIRED) +find_package(OpenSSL REQUIRED) + +include(GNUInstallDirs) + +set(COREUTILS_LIBRARY ATOSCoreUtil) + +add_executable(${PROJECT_NAME} + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/truckobjectcontrol.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../common +) +target_link_libraries(${PROJECT_NAME} + ${COREUTILS_LIBRARY} + OpenSSL::SSL + OpenSSL::Crypto +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + ament_index_cpp +) + +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 + ament_index_cpp +) + +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} +) +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..aefd643ac --- /dev/null +++ b/atos/modules/TruckObjectControl/inc/atostrucksimulator.hpp @@ -0,0 +1,60 @@ +/* + * 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_ = ""; + std::string trajectory_path_name_ = ""; + int start_index_ = 0; + double initial_speed_kmh_ = 0.0; + double target_speed_kmh_ = 40.0; + 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; + + int tcp_fd_ = -1; + std::string tcp_rx_buffer_; + 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 pollTcpCommands(); + void simulationStep(); + 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 new file mode 100644 index 000000000..6283fae16 --- /dev/null +++ b/atos/modules/TruckObjectControl/inc/truckobjectcontrol.hpp @@ -0,0 +1,142 @@ +/* + * 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 +#include +#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_mps = 0.0; + double course_deg = 0.0; + bool tcp_connected = false; + std::string path_name = ""; + 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); + }; + + struct CotObservation { + std::string truck_id; + double distance_along_trajectory_m = 0.0; + double lat = 0.0; + double lon = 0.0; + double speed_mps = 0.0; + double course_deg = 0.0; + bool tcp_connected = false; + std::string path_name = ""; + 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_; + rclcpp::TimerBase::SharedPtr evaluation_timer_; + + 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; + 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"; + 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_ = ""; + bool cot_tls_enabled_ = false; + std::string trajectory_geojson_path_ = ""; + std::string default_path_name_ = ""; + + 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_; + 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; + uint64_t tcp_command_seq_ = 0; + + 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); + bool isCotFresh(const TruckState &state, const rclcpp::Time &now) const; + bool loadTrajectoryPath(); + 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, + int *projected_path_index = nullptr) const; + + void startTcpServer(); + 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/atostrucksimulator.cpp b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp new file mode 100644 index 000000000..8fb71a74d --- /dev/null +++ b/atos/modules/TruckObjectControl/src/atostrucksimulator.cpp @@ -0,0 +1,437 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include + +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; +} + +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; +} + +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") { + 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_); + declare_parameter("ignore_warning_speed_commands", ignore_warning_speed_commands_); + + 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(); + 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(); + 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()); + 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, 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() { + 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; + } + + const int flags = fcntl(tcp_fd_, F_GETFL, 0); + if (flags >= 0) { + (void)fcntl(tcp_fd_, F_SETFL, flags | O_NONBLOCK); + } + + return true; +} + +void AtosTruckSimulator::closeTcp() { + if (tcp_fd_ >= 0) { + ::shutdown(tcp_fd_, SHUT_RDWR); + ::close(tcp_fd_); + tcp_fd_ = -1; + tcp_rx_buffer_.clear(); + } +} + +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, + int &path_index) 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); + + path_index = static_cast(t < 0.5 ? (segment_index - 1) : segment_index); + 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; + int path_index = 0; + if (!pointAtDistance(current_distance_m_, lat, lon, course_deg, path_index)) { + 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; + } + pollTcpCommands(); + + 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 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) { + 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; + } + + 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; + } + 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/main.cpp b/atos/modules/TruckObjectControl/src/main.cpp new file mode 100644 index 000000000..a0053b563 --- /dev/null +++ b/atos/modules/TruckObjectControl/src/main.cpp @@ -0,0 +1,16 @@ +/* + * 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 + +int main(int argc, char **argv) { + std::signal(SIGPIPE, SIG_IGN); + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} 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 new file mode 100644 index 000000000..8829979c3 --- /dev/null +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -0,0 +1,1463 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include + +#include + +#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; +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) { + 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; +} + +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 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)) { + return configured_path; + } + + std::vector candidates; + 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" / 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" / kDefaultGeoJsonName); + } catch (...) { + } + + for (const auto &candidate : candidates) { + if (fs::exists(candidate)) { + return candidate.string(); + } + } + return configured_path; +} +} // namespace + +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_); + 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(); + 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(); + 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(); + 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(); + + 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 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() { + stopTcpServer(); + if (ssl_ctx_ != nullptr) { + SSL_CTX_free(ssl_ctx_); + ssl_ctx_ = nullptr; + } +} + +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_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["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; + 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) && !parseCotXml(msg->data, observation)) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "Failed to parse COT payload from ROS topic."); + return; + } + + 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 = 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; + } + + publishTruckState(observation.truck_id, state); +} + +bool TruckObjectControl::parseCotPlaceholder(const std::string &payload, CotObservation &out) const { + 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.at("id"); + if (out.truck_id.empty()) { + return false; + } + + try { + 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_mps") != fields.end()) { + out.speed_mps = std::stod(fields.at("speed_mps")); + } else if (fields.find("speed_kmh") != fields.end()) { + 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; + } + + return true; +} + +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"); + + 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_mps = 0.0; + out.course_deg = 0.0; + if (std::regex_search(payload, m, track_re) && m.size() >= 3) { + try { + // 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; + out.course_deg = 0.0; + } + } + + 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(); + } + + 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_; + } + } + + 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; +} + +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::loadTrajectoryPathFromFile(const std::string &path, std::vector &out) const { + out.clear(); + + std::ifstream input(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; + } + + out.push_back(p); + prev = p; + has_prev = true; + } + + 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 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; + 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 = 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; +} + +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 (cot_tls_enabled_ && !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."); + return; + } + + 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; + 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 (const int fd : tcp_client_fds_) { + ::shutdown(fd, SHUT_RDWR); + ::close(fd); + } + tcp_client_fds_.clear(); + } + { + std::lock_guard lock(tcp_command_mutex_); + 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_) { + if (thread.joinable()) { + thread.join(); + } + } + 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() { + 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()) { + 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)); + + 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); + } + + 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; + (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; + 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_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()); + } +} + +void TruckObjectControl::handleTcpClient(int client_fd, const std::string &peer_name) { + std::string buffer; + 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_) { + 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"); + } + + 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 (ssl_error=%d)", + peer_name.c_str(), + 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_) { + 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))); + } + 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 (%s). Closing this connection only.", + peer_name.c_str(), describeTlsError(ssl_read_error).c_str()); + break; + } + } 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; + } + } + + 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_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_message = xml; + entry.last_tcp_warning = ""; + entry.last_cot_stamp = now(); + state = entry; + } + { + 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; + } + } + + 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()); + } + + disconnectClientSession(session, "TCP client disconnected"); +} + +void TruckObjectControl::evaluateAndPublishSpeedCommand() { + struct ConnectedTruck { + 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; + + { + std::lock_guard lock(state_mutex_); + for (const auto &[id, state] : trucks_) { + 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, 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; + } + + std::unordered_map> by_path; + for (const auto &truck : connected) { + 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; + 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; }); + + 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_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); + } + + const std::string previous_command = group[i].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); + 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 = "DRIVE"; + } + + 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; + } 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_drive_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 + + ";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; + 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; + } + } + + if (!std::isfinite(min_gap_m)) { + min_gap_m = -1.0; + } + + std::string fleet_target_speed_mps_value = "nochange"; + std::string fleet_reason = "no_limit"; + 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 (any_slowdown) { + fleet_target_speed_mps_value = std::to_string(warning_speed_kmh_ / 3.6); + fleet_reason = "min_gap_below_warning_distance"; + } + + std_msgs::msg::String command; + command.data = "target_speed_mps=" + fleet_target_speed_mps_value + + ";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); + + RCLCPP_WARN(get_logger(), + "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::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::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 (target_id.empty() || command.empty()) { + continue; + } + + const std::string payload = command + "\n"; + if (cot_tls_enabled_) { + if (session->ssl == nullptr) { + updateTruckTcpStatus(target_id, command, "TLS session missing for this truck", true); + ::shutdown(session->fd, SHUT_RDWR); + break; + } + + 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; + } + + 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; + } + + 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 (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, + 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->sender_thread.joinable()) { + session->sender_thread.join(); + } + + 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; + } + + { + 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; + { + 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) { + updateTruckTcpStatus(target_id, command, "No active TCP socket for this truck"); + 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; + } + } + + 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(); +} diff --git a/atos_gui/atos_gui/main.py b/atos_gui/atos_gui/main.py index e438f9707..964c84787 100644 --- a/atos_gui/atos_gui/main.py +++ b/atos_gui/atos_gui/main.py @@ -3,72 +3,295 @@ file, You can obtain one at https://mozilla.org/MPL/2.0/. """ +import json +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 rclpy.node import Node +from std_msgs.msg import String + 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" +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_MAP_JS_VERSION = "20260424-3" +FLEET_STATE_LOCK = threading.Lock() +FLEET_TRUCK_STATES: dict[str, dict] = {} +FLEET_GEOJSON_CACHE: dict[str, dict] = {} -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 _candidate_conf_dirs() -> list[Path]: + candidates = [ + 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") + 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 _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 + + 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()) + + path_names = {GEOJSON_NAME} + for truck in trucks: + path_name = truck.get("path_name") + if path_name: + path_names.add(Path(str(path_name)).name) + + paths_payload = {} + for path_name in sorted(path_names): + geojson, _ = _load_geojson_by_name(path_name) + if geojson: + paths_payload[path_name] = geojson + + payload = { + "default_path_name": GEOJSON_NAME, + "paths": paths_payload, + "trucks": trucks, + } + return json.dumps(payload).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 + + if payload.get("path_name"): + payload["path_name"] = Path(str(payload["path_name"])).name + + with FLEET_STATE_LOCK: + FLEET_TRUCK_STATES[str(uid)] = payload + + +def render_atosfleetmanagement_pages() -> None: + 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") + + 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: +- `truck_object_control` +- `foxglove_bridge` / `rosbridge` + +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` + +Live truck state topic for map overlay: +- `/atos/truck_objects/state` + """.strip() + ) + + with ui.tab_panel(road_tab): + default_geojson, source_path = _load_geojson_by_name(GEOJSON_NAME) + ui.label("Path visualization by COT path_name").classes("text-h5") + + if not default_geojson: + ui.label("Could not load default geojson file.").classes("text-red-600") + ui.markdown("Searched conf dirs:\n" + "\n".join([f"- `{p}`" for p in _candidate_conf_dirs()])) + return + + ui.label(f"Default source: {source_path}").classes("text-sm text-gray-600") + map_id = "fleet-road-map" + ui.html(f'
') + + ui.timer( + 0.2, + lambda: ui.run_javascript( + f""" +(() => {{ + const payload = {_fleet_map_payload_json()}; + 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); + 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.updateFleetRoadMap('{map_id}', {_fleet_map_payload_json()});" + ), + ) + + 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 FLEET_MODE: + render_atosfleetmanagement_pages() + nodes = [FleetStateNode()] + 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() 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:8420", 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()), + "host": "0.0.0.0", + "port": 8420, + "show": False, + "title": "TruckObjectGUI" if FLEET_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) + print("ATTENTION: Using SSL, use https://localhost:8420 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/atos_gui/static/fleet_map.js b/atos_gui/atos_gui/static/fleet_map.js new file mode 100644 index 000000000..4e124dfee --- /dev/null +++ b/atos_gui/atos_gui/static/fleet_map.js @@ -0,0 +1,411 @@ +(function() { + function toRad(value) { + 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) { + 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)); + }).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; + }).sort(function(a, b) { + return a.uid.localeCompare(b.uid, undefined, { sensitivity: "base", numeric: true }); + }); + } + + 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) { + const itemPath = item.path_name || selectedPathName; + return itemPath === selectedPathName && 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.01) { + result[current.uid] = gap; + } + } + return result; + } + + function renderSvg(container, coords, trucks, selectedPathName, pathsByName) { + const width = 600; + const height = 650; + 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 filteredTrucks = trucks.filter(function(item) { + const itemPath = item.path_name || selectedPathName; + return itemPath === selectedPathName; + }); + + const aheadDistanceMap = computeAheadDistanceMap(filteredTrucks, totalMeters, selectedPathName); + + 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 = + Number.isFinite(ahead) + ? (" next: " + ahead.toFixed(1) + " m") + : " next: -"; + return ( + "" + + "" + + "" + + uid + " [idx " + pathIndex + "] " + speedKmh.toFixed(1) + " km/h @" + courseDeg.toFixed(0) + "°" + aheadText + + "" + + "" + ); + }).join(""); + + 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 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) + ? ahead.toFixed(1) + " m" + : "-"; + return ( + "" + + "" + uid + "" + + "" + pathIndex + "" + + "" + speedKmh.toFixed(1) + " km/h" + + "" + courseDeg.toFixed(0) + "°" + + "" + aheadCell + "" + + "" + + "
Latest CoT (Rx)
" + + "
" + escapeHtml(lastCotMessage) + "
" + + "
Latest TCP (Tx)
" + + "
" + escapeHtml(lastTcpCommand) + "
" + + (tcpWarning ? "
TCP warning: " + escapeHtml(tcpWarning) + "
" : "") + + "" + + "" + ); + }).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 = + "
    " + + "
    " + + "" + + "" + + truckCircles + + "" + + "
    " + + "
    Selected path: " + selectedPathName + "
    " + + "
    Path points: " + coords.length + "
    " + + "
    Total length (Vincenty): " + totalMeters.toFixed(2) + " m
    " + + "
    Total length: " + totalKm.toFixed(3) + " km
    " + + "
    Live trucks on selected path: " + filteredTrucks.length + "
    " + + "
    Available paths in payload
    " + + "
      " + pathSummary + "
    " + + "
    " + + "
    " + + "
    " + + "
    Distance To Next Truck Ahead
    " + + "" + + "" + + "" + + "" + + "" + + "" + + "" + + "" + truckRows + "
    TruckPath idxSpeedCourseNext aheadLatest CoT (Rx) / Latest TCP (Tx)
    " + + "
    " + + "
    "; + } + + function renderInternal(containerId) { + const container = document.getElementById(containerId); + if (!container) { + return; + } + const state = window.__fleetRoadMapState[containerId]; + if (!state) { + return; + } + + 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, 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, payload) { + const p = normalizePayload(payload); + window.__fleetRoadMapState[containerId] = { + defaultPathName: p.default_path_name, + pathsByName: p.paths, + trucks: p.trucks, + }; + renderInternal(containerId); + }; + + 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); + }; +})(); diff --git a/atos_gui/setup.py b/atos_gui/setup.py index 44ae0fab7..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, @@ -21,7 +24,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 +) 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": {} + } + ] +} diff --git a/docker-compose-fleetmanagement.yml b/docker-compose-fleetmanagement.yml new file mode 100644 index 000000000..04d819e43 --- /dev/null +++ b/docker-compose-fleetmanagement.yml @@ -0,0 +1,26 @@ +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} + - 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" + command: /bin/bash -lc "/root/atos_git/scripts/run_atosfleetmanagement.sh" 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/atosfleetmanagement.env.example b/scripts/atosfleetmanagement.env.example new file mode 100644 index 000000000..4c3526f03 --- /dev/null +++ b/scripts/atosfleetmanagement.env.example @@ -0,0 +1,9 @@ +# Copy this file to /etc/default/atosfleetmanagement and adjust values. +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/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/build_atosfleetmanagement.sh b/scripts/build_atosfleetmanagement.sh new file mode 100755 index 000000000..a8431d345 --- /dev/null +++ b/scripts/build_atosfleetmanagement.sh @@ -0,0 +1,70 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Canonical build profile for ATOSFleetManagement. +# Default workspace: ~/atos_ws +# Usage: +# ./scripts/build_atosfleetmanagement.sh +# ./scripts/build_atosfleetmanagement.sh /path/to/ws +# ATOS_WS=/path/to/ws ./scripts/build_atosfleetmanagement.sh --event-handlers console_direct+ + +if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then + cat <<'HELP' +ATOSFleetManagement build script + +Usage: + scripts/build_atosfleetmanagement.sh [workspace] [extra colcon args...] + +Examples: + scripts/build_atosfleetmanagement.sh + scripts/build_atosfleetmanagement.sh /home/user/atos_ws + scripts/build_atosfleetmanagement.sh --event-handlers console_direct+ + scripts/build_atosfleetmanagement.sh /home/user/atos_ws --executor sequential + +Behavior: + - Builds packages: atos, atos_gui + - Enables TruckObjectControl module + - Disables legacy ATOS modules not used in ATOSFleetManagement +HELP + exit 0 +fi + +DEFAULT_WS="${ATOS_WS:-$HOME/atos_ws}" +WS="$DEFAULT_WS" + +# If first arg is not an option, treat it as workspace path. +if [[ $# -gt 0 && "${1:0:1}" != "-" ]]; then + WS="$1" + shift +fi + +if [[ ! -d "$WS" ]]; then + echo "Workspace does not exist: $WS" + echo "Create it first or pass a valid workspace path." + exit 1 +fi + +if [[ ! -d "$WS/src" ]]; then + echo "Workspace is missing src directory: $WS/src" + exit 1 +fi + +echo "Building ATOSFleetManagement in workspace: $WS" + +cd "$WS" +colcon build --packages-select atos atos_gui --symlink-install --cmake-args \ + -DWITH_TRUCK_OBJECT_CONTROL=ON \ + -DWITH_OBJECT_CONTROL=OFF \ + -DWITH_OPEN_SCENARIO_GATEWAY=OFF \ + -DWITH_JOURNAL_CONTROL=OFF \ + -DWITH_ESMINI_ADAPTER=OFF \ + -DWITH_DIRECT_CONTROL=OFF \ + -DWITH_TRAJECTORYLET_STREAMER=OFF \ + -DWITH_OSI_ADAPTER=OFF \ + -DWITH_MQTT_BRIDGE=OFF \ + -DWITH_POINTCLOUD_PUBLISHER=OFF \ + -DWITH_INTEGRATION_TESTING=OFF \ + -DWITH_BACK_TO_START=OFF \ + -DWITH_REST_BRIDGE=OFF \ + -DWITH_MONR_RELAY=OFF \ + "$@" 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..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 #################### ################################################ @@ -32,12 +47,18 @@ 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 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." @@ -47,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}" ;; @@ -63,4 +92,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/installation/install_deps.sh b/scripts/installation/install_deps.sh index 10385c45f..2bb464667 100755 --- a/scripts/installation/install_deps.sh +++ b/scripts/installation/install_deps.sh @@ -19,11 +19,84 @@ 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 + 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} -python3 -m pip install -r ${ATOS_REPO_PATH}/scripts/installation/requirements.txt +apt_update_retry +apt_install_retry ${apt_deps} +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." @@ -31,14 +104,23 @@ 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 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 # Authorize the ROS2 gpg key with apt sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ @@ -52,7 +134,9 @@ fi # Install ROS2 packages echo "Installing ROS2 packages..." -sudo apt install -y \ +apt_update_retry +apt_install_retry \ + ros-dev-tools \ ros-${ROS_DISTRO}-desktop \ python3-rosdep \ ros-${ROS_DISTRO}-launch-pytest @@ -67,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 ###### @@ -105,7 +189,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 @@ -121,4 +205,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." diff --git a/scripts/installation/install_functions.sh b/scripts/installation/install_functions.sh index 9f8601a07..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,12 +91,12 @@ 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 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 +110,4 @@ add_source_line_if_needed() { echo "$source_line" >> "$file" fi fi -} \ No newline at end of file +} diff --git a/scripts/run_atosfleetmanagement.sh b/scripts/run_atosfleetmanagement.sh new file mode 100644 index 000000000..f3534925a --- /dev/null +++ b/scripts/run_atosfleetmanagement.sh @@ -0,0 +1,40 @@ +#!/usr/bin/env bash + +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}" + +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[@]}" 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" 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"