Feature atos mini prep

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
+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"]

README.md

@@ -30,22 +30,93 @@ 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. 
 You might wish to mount the config directory at ~/.astazero/ATOS/ to a different location on your host computer. This can be done by changing the path after the -v flag in the above command. You might also wish to inspect the image with instead of running the launch_basic.py script. This can be done by removing the last "bash -c ..." part of the command.
 
 
 ## <a name="Installation script"></a> 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
 ```
 
 ## <a name="Native build"></a> 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=<truck_id>;distance_m=<value>;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
+```
+
 # <a name="usage"></a> 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
 </picture>
 <br>
 <br>
-

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

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:

atos/common/roschannels/roschannel.hpp

@@ -19,6 +19,7 @@ class BasePub {
         const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepAll()))
         : pub(node.create_publisher<T>(topicName, qos)) {}
     BasePub() = delete;
+    virtual ~BasePub() = default;
     typename rclcpp::Publisher<T>::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<T>(topicName, qos, callback)) {}
     BaseSub() = delete;
+    virtual ~BaseSub() = default;
     typename rclcpp::Subscription<T>::SharedPtr sub;
 };
 
-} // namespace ROSChannels
+} // namespace ROSChannels

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);

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,
+        ),
+    ])

atos/modules/DirectControl/src/directcontrol.cpp

@@ -178,7 +178,7 @@ void DirectControl::readTCPSocketData() {
 					try {
 						this->handleISOMessage(data, static_cast<size_t>(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);
 					}
 				}