From 48ab5ff592e0a01d46a08e8c63ea85fc74f4e223 Mon Sep 17 00:00:00 2001 From: Stefan Date: Tue, 14 Apr 2026 10:45:34 +0200 Subject: [PATCH 01/12] RemoteControl APP improvements (#231) * fix(APPRemoteControl): add ROBOT_SET channel and fix initial pose handling * chore: remove default PIO envs * docs: fix typos * fix(APPRemoteControl): improve logs --- .../Platoon/V2VCommManagerClass.puml | 4 +- lib/APPRemoteControl/src/App.cpp | 41 ++++++++++++++----- lib/APPRemoteControl/src/App.h | 10 +++-- lib/PlatoonService/src/V2VCommManager.cpp | 14 +++---- lib/Utilities/src/SimpleTimer.hpp | 2 +- platformio.ini | 2 +- 6 files changed, 49 insertions(+), 24 deletions(-) diff --git a/doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml b/doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml index d4b2b6ef..70f7f0e9 100644 --- a/doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml +++ b/doc/architecture/uml/LogicalView/Platoon/V2VCommManagerClass.puml @@ -23,11 +23,11 @@ package "PlatoonService" as serv { package "HAL" as hal { class "MQTT Client" as mqtt { + publish(String topic, String payload) : bool - + subcribe(String topic, TopicCallback callback) : bool + + subscribe(String topic, TopicCallback callback) : bool } } app *--> VCM : <> VCM ..> mqtt : <> -@enduml \ No newline at end of file +@enduml diff --git a/lib/APPRemoteControl/src/App.cpp b/lib/APPRemoteControl/src/App.cpp index eeb6a0db..a95df11c 100644 --- a/lib/APPRemoteControl/src/App.cpp +++ b/lib/APPRemoteControl/src/App.cpp @@ -178,7 +178,7 @@ void App::loop() /* Process SerialMuxProt. */ m_smpServer.process(millis()); - if (false == m_initialDataSent) + if ((false == m_initialDataSent) && (true == m_smpServer.isSynced())) { SettingsHandler& settings = SettingsHandler::getInstance(); Command cmd = {SMPChannelPayload::CMD_ID_SET_INIT_POS, settings.getInitialXPosition(), @@ -189,6 +189,10 @@ void App::loop() LOG_DEBUG("Initial vehicle data sent."); m_initialDataSent = true; } + else + { + LOG_WARNING("Failed to send initial vehicle data."); + } } if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced())) @@ -231,6 +235,8 @@ bool App::setupSerialMuxProtServer() m_serialMuxProtChannelIdRemoteCtrl = m_smpServer.createChannel(COMMAND_CHANNEL_NAME, COMMAND_CHANNEL_DLC); m_serialMuxProtChannelIdMotorSpeeds = m_smpServer.createChannel(MOTOR_SPEED_SETPOINT_CHANNEL_NAME, MOTOR_SPEED_SETPOINT_CHANNEL_DLC); + m_serialMuxProtChannelIdRobotSpeeds = + m_smpServer.createChannel(ROBOT_SPEED_SETPOINT_CHANNEL_NAME, ROBOT_SPEED_SETPOINT_CHANNEL_DLC); m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC); m_smpServer.subscribeToChannel(COMMAND_RESPONSE_CHANNEL_NAME, App_cmdRspChannelCallback); @@ -238,7 +244,7 @@ bool App::setupSerialMuxProtServer() m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_NAME, App_currentVehicleChannelCallback); if ((0U == m_serialMuxProtChannelIdRemoteCtrl) || (0U == m_serialMuxProtChannelIdMotorSpeeds) || - (0U == m_serialMuxProtChannelIdStatus)) + (0U == m_serialMuxProtChannelIdRobotSpeeds) || (0U == m_serialMuxProtChannelIdStatus)) { LOG_ERROR("Failed to create SerialMuxProt channels."); } @@ -278,13 +284,13 @@ bool App::setupMqtt(const String& clientId, const String& brokerAddr, uint16_t b else if (false == m_mqttClient.subscribe(TOPIC_NAME_CMD, true, [this](const String& payload) { cmdTopicCallback(payload); })) { - LOG_FATAL("Could not subcribe to MQTT topic: %s.", TOPIC_NAME_CMD); + LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_CMD); } /* Subscribe to Motor Speeds Topic. */ else if (false == m_mqttClient.subscribe(TOPIC_NAME_MOTOR_SPEEDS, true, [this](const String& payload) { motorSpeedsTopicCallback(payload); })) { - LOG_FATAL("Could not subcribe to MQTT topic: %s.", TOPIC_NAME_MOTOR_SPEEDS); + LOG_FATAL("Could not subscribe to MQTT topic: %s.", TOPIC_NAME_MOTOR_SPEEDS); } else { @@ -341,10 +347,25 @@ void App::cmdTopicCallback(const String& payload) break; case 6U: - cmd.commandId = SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS; - LOG_WARNING("Setting initial position is not supported."); - isValid = false; + { + JsonVariantConst xPos = jsonPayload["X"]; + JsonVariantConst yPos = jsonPayload["Y"]; + JsonVariantConst heading = jsonPayload["HEADING"]; + + if (xPos.isNull() || yPos.isNull() || heading.isNull()) + { + LOG_WARNING("CMD_ID_SET_INIT_POS requires X, Y and HEADING fields."); + isValid = false; + } + else + { + cmd.commandId = SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS; + cmd.xPos = xPos.as(); + cmd.yPos = yPos.as(); + cmd.orientation = heading.as(); + } break; + } default: isValid = false; @@ -353,16 +374,16 @@ void App::cmdTopicCallback(const String& payload) if (false == isValid) { - LOG_ERROR("Invalid command ID %d.", cmdId); + LOG_ERROR("Got invalid payload in command with ID %d.", cmdId); } else if (true == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, reinterpret_cast(&cmd), sizeof(cmd))) { - LOG_DEBUG("Command %d sent.", cmd.commandId); + LOG_DEBUG("Command with ID %d successfully sent.", cmd.commandId); } else { - LOG_WARNING("Failed to send command %d.", cmd.commandId); + LOG_WARNING("Failed to send command with ID %d.", cmd.commandId); } } else diff --git a/lib/APPRemoteControl/src/App.h b/lib/APPRemoteControl/src/App.h index 4683665c..b8e8f938 100644 --- a/lib/APPRemoteControl/src/App.h +++ b/lib/APPRemoteControl/src/App.h @@ -70,6 +70,7 @@ class App m_smpServer(Board::getInstance().getRobot().getStream()), m_serialMuxProtChannelIdRemoteCtrl(0U), m_serialMuxProtChannelIdMotorSpeeds(0U), + m_serialMuxProtChannelIdRobotSpeeds(0U), m_serialMuxProtChannelIdStatus(0U), m_mqttClient(), m_initialDataSent(false), @@ -109,13 +110,16 @@ class App /** MQTT topic name for receiving motor speeds. */ static const char* TOPIC_NAME_MOTOR_SPEEDS; - /** SerialMuxProt Channel id for sending remote control commands. */ + /** SerialMuxProt Channel ID for sending remote control commands. */ uint8_t m_serialMuxProtChannelIdRemoteCtrl; - /** SerialMuxProt Channel id for sending motor speeds. */ + /** SerialMuxProt Channel ID for sending motor speeds. */ uint8_t m_serialMuxProtChannelIdMotorSpeeds; - /** SerialMuxProt Channel id for sending system status. */ + /** SerialMuxProt Channel ID for sending robot speed setpoints (linear + angular). */ + uint8_t m_serialMuxProtChannelIdRobotSpeeds; + + /** SerialMuxProt Channel ID for sending system status. */ uint8_t m_serialMuxProtChannelIdStatus; /** diff --git a/lib/PlatoonService/src/V2VCommManager.cpp b/lib/PlatoonService/src/V2VCommManager.cpp index 909e04da..a26b3868 100644 --- a/lib/PlatoonService/src/V2VCommManager.cpp +++ b/lib/PlatoonService/src/V2VCommManager.cpp @@ -611,12 +611,12 @@ bool V2VCommManager::setupCommonTopics(uint8_t platoonId, uint8_t vehicleId) /* Subscribe to Input Topic. */ if (false == m_mqttClient.subscribe(m_waypointInputTopic, false, lambdaWaypointInputTopicCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_waypointInputTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_waypointInputTopic.c_str()); } /* Subscribe to emergency topic. */ else if (false == m_mqttClient.subscribe(m_emergencyTopic, false, lambdaWaypointInputTopicCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_emergencyTopic); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_emergencyTopic); } else { @@ -667,7 +667,7 @@ bool V2VCommManager::setupHeartbeatTopics(uint8_t platoonId, uint8_t vehicleId) /* Subscribe to platoon heartbeat Topic. */ if (false == m_mqttClient.subscribe(m_platoonHeartbeatTopic, false, lambdaHeartbeatInputTopicCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); } { isSuccessful = true; @@ -700,17 +700,17 @@ bool V2VCommManager::setupLeaderTopics() /* Subscribe to Vehicles Heartbeat Topics. */ else if (false == m_mqttClient.subscribe(m_heartbeatResponseTopic, false, lambdaEventCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_platoonHeartbeatTopic.c_str()); } /* Subscribe to Last Vehicle Feedback Topic. */ else if (false == m_mqttClient.subscribe(m_feedbackTopic, false, lambdaEventCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_feedbackTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_feedbackTopic.c_str()); } /* Subscribe to IVS Topic. */ else if (false == m_mqttClient.subscribe(m_ivsTopic, false, lambdaEventCallback)) { - LOG_ERROR("Could not subcribe to MQTT Topic: %s.", m_ivsTopic.c_str()); + LOG_ERROR("Could not subscribe to MQTT Topic: %s.", m_ivsTopic.c_str()); } else { @@ -867,4 +867,4 @@ bool V2VCommManager::sendPlatoonLength(const int32_t length) const /****************************************************************************** * Local Functions - *****************************************************************************/ \ No newline at end of file + *****************************************************************************/ diff --git a/lib/Utilities/src/SimpleTimer.hpp b/lib/Utilities/src/SimpleTimer.hpp index 8bee845f..2431049d 100644 --- a/lib/Utilities/src/SimpleTimer.hpp +++ b/lib/Utilities/src/SimpleTimer.hpp @@ -192,7 +192,7 @@ class SimpleTimer /** * Get current duration in ms, till the timer was started. - * It is independed of whether the timer is stopped or timeout. + * It is independent of whether the timer is stopped or timeout. * * @return Current duration in ms */ diff --git a/platformio.ini b/platformio.ini index cc04220a..7d71fe9d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,7 +12,7 @@ ; PlatformIO specific configurations ; ***************************************************************************** [platformio] -default_envs = ConvoyLeaderTarget, ConvoyLeaderSim +;default_envs = ConvoyLeaderTarget, ConvoyLeaderSim extra_configs = config/buildmode.ini From 0eec9a7b1d465c7d2c646a26db577c27510851e4 Mon Sep 17 00:00:00 2001 From: Stefan Vogel Date: Tue, 12 May 2026 15:15:54 +0200 Subject: [PATCH 02/12] docs: add tcp WSL hint --- platformio.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platformio.ini b/platformio.ini index 7d71fe9d..6091bd54 100644 --- a/platformio.ini +++ b/platformio.ini @@ -102,8 +102,8 @@ extra_scripts = post:./scripts/copy_webots_shared_libs.py ; Custom options (https://docs.platformio.org/en/latest/scripting/examples/platformio_ini_custom_options.html) -custom_webots_ip_address = 127.0.0.1 ; IP address of the Webots simulation, which is used for TCP communication custom_webots_protocol = ipc ; [ipc|tcp] - ipc is faster but only works on the same machine, tcp works also over network +custom_webots_ip_address = 127.0.0.1 ; IP address of the Webots simulation (only used for TCP protocol). Under WSL run 'ip route show | grep default' to find out. custom_webots_robot_name = ZumoComSystem custom_webots_robot_serial_rx_channel = 4 custom_webots_robot_serial_tx_channel = 3 From 99c3589c33ad44807a0ad7c15237f3fd16db47e5 Mon Sep 17 00:00:00 2001 From: Stefan Vogel Date: Wed, 13 May 2026 07:01:23 +0200 Subject: [PATCH 03/12] feat: add MQTT keyboard control python client for DCS --- data/config/config.json | 4 +- scripts/keyboard_control.py | 181 ++++++++++++++++++++++++++++++++++++ scripts/requirements.txt | 1 + 3 files changed, 184 insertions(+), 2 deletions(-) create mode 100755 scripts/keyboard_control.py create mode 100644 scripts/requirements.txt diff --git a/data/config/config.json b/data/config/config.json index 5601d32b..c0fd9eca 100644 --- a/data/config/config.json +++ b/data/config/config.json @@ -1,5 +1,5 @@ { - "robotName": "", + "robotName": "Zumo", "wifi": { "ssid": "", "pswd": "" @@ -29,4 +29,4 @@ "host": "127.0.0.1", "port": "8888" } -} \ No newline at end of file +} diff --git a/scripts/keyboard_control.py b/scripts/keyboard_control.py new file mode 100755 index 00000000..2ebffe1c --- /dev/null +++ b/scripts/keyboard_control.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python3 +""" +Keyboard remote control for the Zumo robot via MQTT. + +Controls: + W / Up - Forward + S / Down - Backward + A / Left - Turn left + D / Right - Turn right + Space - Stop + Q / Esc - Quit + +Usage: + python3 keyboard_control.py [--broker ] [--port ] [--robot ] [--speed ] + +The robot name must match the DCS config (data/config/config.json "robotName"). +If robotName is empty in the config, DCS derives it from its process ID — set a +fixed name in the config and pass it here. +""" + +import argparse +import json +import sys +import termios +import tty + +import paho.mqtt.client as mqtt + +DEFAULT_BROKER = "localhost" +DEFAULT_PORT = 1883 +DEFAULT_ROBOT = "Zumo" +DEFAULT_SPEED = 100 # mm/s + +TOPIC_MOTOR_SPEEDS = "{robot}/motorSpeeds" +TOPIC_CMD = "{robot}/cmd" + +CMD_ID_START_DRIVING = 5 + + +def parse_args(): + """Parse command-line arguments and return the populated namespace.""" + p = argparse.ArgumentParser( + description="Keyboard MQTT remote control for Zumo robot") + p.add_argument("--broker", default=DEFAULT_BROKER, help="MQTT broker host") + p.add_argument("--port", type=int, default=DEFAULT_PORT, + help="MQTT broker port") + p.add_argument("--robot", default=DEFAULT_ROBOT, + help="Robot name (MQTT client ID prefix)") + p.add_argument("--speed", type=int, default=DEFAULT_SPEED, + help="Drive speed in mm/s") + return p.parse_args() + + +def publish_motor_speeds(client, topic, left, right): + """Publish left/right motor speeds (mm/s) as JSON to the given MQTT topic.""" + payload = json.dumps({"LEFT": left, "RIGHT": right}) + client.publish(topic, payload) + + +def publish_start_driving(client, cmd_topic): + """Send CMD_ID_START_DRIVING to the robot's command topic, enabling motor control.""" + payload = json.dumps({"CMD_ID": CMD_ID_START_DRIVING}) + client.publish(cmd_topic, payload) + + +def on_connect(client, userdata, flags, reason_code, properties): + """MQTT on-connect callback — sends START_DRIVING on success, exits on failure.""" + if reason_code == 0: + print( + f"Connected to MQTT broker at {userdata['broker']}:{userdata['port']}") + # Tell DCS to enter driving mode + publish_start_driving(client, userdata["cmd_topic"]) + else: + print(f"Connection failed with code {reason_code}", file=sys.stderr) + sys.exit(1) + + +def get_char(): + """Read a single character from stdin without requiring Enter.""" + fd = sys.stdin.fileno() + old = termios.tcgetattr(fd) + try: + tty.setraw(fd) + ch = sys.stdin.read(1) + # Handle escape sequences (arrow keys) + if ch == "\x1b": + ch2 = sys.stdin.read(1) + if ch2 == "[": + ch3 = sys.stdin.read(1) + return "\x1b[" + ch3 + return ch + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old) + + +def main(): + """Connect to the MQTT broker and run the keyboard control loop until quit.""" + args = parse_args() + + motor_topic = TOPIC_MOTOR_SPEEDS.format(robot=args.robot) + cmd_topic = TOPIC_CMD.format(robot=args.robot) + + client = mqtt.Client( + mqtt.CallbackAPIVersion.VERSION2, + client_id=f"keyboard_control_{args.robot}", + ) + client.user_data_set({ + "broker": args.broker, + "port": args.port, + "cmd_topic": cmd_topic, + }) + client.on_connect = on_connect + + try: + client.connect(args.broker, args.port, keepalive=60) + except Exception as e: + print(f"Cannot connect to broker: {e}", file=sys.stderr) + sys.exit(1) + + client.loop_start() + + speed = args.speed + + print() + print("=== Zumo Keyboard Control ===") + print(f" Robot : {args.robot}") + print(f" Broker: {args.broker}:{args.port}") + print(f" Speed : {speed} mm/s") + print() + print(" W/Up – Forward") + print(" S/Down – Backward") + print(" A/Left – Turn left (in place)") + print(" D/Right – Turn right (in place)") + print(" Space – Stop") + print(" Q/Esc – Quit") + print() + + try: + current = (0, 0) + + while True: + ch = get_char().lower() + + if ch in ("q", "\x1b"): + break + + if ch in ("w", "\x1b[a"): # W or Up arrow + left, right = speed, speed + label = "FORWARD" + elif ch in ("s", "\x1b[b"): # S or Down arrow + left, right = -speed, -speed + label = "BACKWARD" + elif ch in ("a", "\x1b[d"): # A or Left arrow + left, right = -speed, speed + label = "LEFT" + elif ch in ("d", "\x1b[c"): # D or Right arrow + left, right = speed, -speed + label = "RIGHT" + elif ch == " ": + left, right = 0, 0 + label = "STOP" + else: + continue + + if (left, right) != current: + current = (left, right) + publish_motor_speeds(client, motor_topic, left, right) + print( + f"\r [{label}] L={left:+5d} mm/s R={right:+5d} mm/s ", end="", flush=True) + + except KeyboardInterrupt: + pass + finally: + publish_motor_speeds(client, motor_topic, 0, 0) + client.loop_stop() + client.disconnect() + print("\nStopped.") + + +if __name__ == "__main__": + main() diff --git a/scripts/requirements.txt b/scripts/requirements.txt new file mode 100644 index 00000000..8cc4a62f --- /dev/null +++ b/scripts/requirements.txt @@ -0,0 +1 @@ +paho-mqtt>=2.0 From 4e43b18a9af85920e2b10f4b3a5e05403b6b427e Mon Sep 17 00:00:00 2001 From: Stefan Vogel Date: Wed, 13 May 2026 07:08:54 +0200 Subject: [PATCH 04/12] feat: add webots not running check --- scripts/webots_launcher.py | 50 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/scripts/webots_launcher.py b/scripts/webots_launcher.py index 9ee12aa3..39fd7f88 100644 --- a/scripts/webots_launcher.py +++ b/scripts/webots_launcher.py @@ -98,6 +98,55 @@ # Functions ################################################################################ +def _abort_missing_slot(protocol, ip, robot_name): + """Check the robot has an extern slot in Webots, print a clear error and exit if not.""" + import socket + + if protocol == "tcp": + try: + s = socket.create_connection((ip, 1234), timeout=2) + s.close() + except OSError: + print(f"Error: Cannot reach Webots at {ip}:1234 via TCP. Is Webots running?") + raise SystemExit(1) + print(f"Warning: Cannot verify extern slot for '{robot_name}' over TCP — " + "make sure the correct world is loaded.") + return + + import getpass + ipc_dir = f"/tmp/webots/{getpass.getuser()}/1234/ipc" + if not os.path.isdir(ipc_dir): + print("Error: Webots is not running or no world is loaded.") + raise SystemExit(1) + + ipc_socket = f"{ipc_dir}/{robot_name}/extern" + if not os.path.exists(ipc_socket): + available = [n for n in os.listdir(ipc_dir) + if os.path.exists(os.path.join(ipc_dir, n, "extern"))] + slots = ", ".join(available) if available else "(none)" + print(f"Error: Robot '{robot_name}' has no extern controller slot in the loaded world.") + print(f" Available extern slots: {slots}") + print(" Load a world where the robot has controller \"\".") + raise SystemExit(1) + + try: + s = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + s.settimeout(1) + s.connect(ipc_socket) + s.close() + except OSError: + print(f"Error: Extern slot for '{robot_name}' exists but is already claimed by another controller.") + raise SystemExit(1) + + +def check_webots_ready(source, target, env): # pylint: disable=unused-argument + """Check that Webots is running and the robot has a free extern controller slot.""" + protocol = env.GetProjectOption("custom_webots_protocol") + ip = env.GetProjectOption("custom_webots_ip_address") + robot_name = env.GetProjectOption("custom_webots_robot_name") + _abort_missing_slot(protocol, ip, robot_name) + + ################################################################################ # Main ################################################################################ @@ -107,6 +156,7 @@ name="webots_launcher", dependencies=PROGRAM_PATH + PROGRAM_NAME, actions=[ + check_webots_ready, WEBOTS_LAUNCHER_ACTION ], title="Launch", From f379544729c35cd41f1dbf4a1636bb13d8f33ccc Mon Sep 17 00:00:00 2001 From: Stefan Vogel Date: Wed, 13 May 2026 07:28:45 +0200 Subject: [PATCH 05/12] fix(sim): tolerate webots-controller SIGSEGV on TCP disconnect webots-controller crashes with SIGSEGV (exit 139) in libController.so when the remote Webots host closes the TCP connection. Replace the raw shell action with a subprocess wrapper that treats exit code 139 as success, since the DCS process itself exits cleanly at that point. Also replace `return status` with `exit(status)` in main() to bypass C++ static destructor teardown: ~webots::Robot() calls wb_robot_cleanup() and ~MqttClient() calls mosquitto_loop_stop(), both of which crash or block when the Webots connection is already gone. --- lib/MainNative/src/main.cpp | 7 ++++++- scripts/webots_launcher.py | 19 +++++++++++++++++-- 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/lib/MainNative/src/main.cpp b/lib/MainNative/src/main.cpp index 8f5db92f..94196e72 100644 --- a/lib/MainNative/src/main.cpp +++ b/lib/MainNative/src/main.cpp @@ -291,7 +291,12 @@ extern int main(int argc, char** argv) } } - return status; + /* Use exit() instead of return to skip C++ static destructor teardown. + * ~webots::Robot() calls wb_robot_cleanup() and ~MqttClient() calls + * mosquitto_loop_stop(), both of which crash or block when Webots has + * already closed the connection. + */ + exit(status); } /****************************************************************************** diff --git a/scripts/webots_launcher.py b/scripts/webots_launcher.py index 39fd7f88..a6fa1191 100644 --- a/scripts/webots_launcher.py +++ b/scripts/webots_launcher.py @@ -85,7 +85,7 @@ print(f"OS type {OS_PLATFORM_TYPE} not supported.") sys.exit(1) -WEBOTS_LAUNCHER_ACTION = WEBOTS_CONTROLLER + ' '\ +WEBOTS_LAUNCHER_CMD = WEBOTS_CONTROLLER + ' '\ + WEBOTS_CONTROLLER_OPTIONS + ' ' \ + PROGRAM_PATH + PROGRAM_NAME + ' ' \ + PROGRAM_OPTIONS @@ -147,6 +147,21 @@ def check_webots_ready(source, target, env): # pylint: disable=unused-argument _abort_missing_slot(protocol, ip, robot_name) +def launch_webots_controller(source, target, env): # pylint: disable=unused-argument + """Run webots-controller, tolerating SIGSEGV (exit 139) on TCP disconnect. + + webots-controller segfaults in libController.so when the remote Webots host + closes the TCP connection. The DCS process itself exits cleanly; the crash + is inside the launcher binary and is harmless. + """ + import subprocess + cmd = env.subst(WEBOTS_LAUNCHER_CMD) + print(cmd) + result = subprocess.run(cmd, shell=True) + if result.returncode not in (0, -11, 139): # 139 = 128 + SIGSEGV(11) + env.Exit(result.returncode) + + ################################################################################ # Main ################################################################################ @@ -157,7 +172,7 @@ def check_webots_ready(source, target, env): # pylint: disable=unused-argument dependencies=PROGRAM_PATH + PROGRAM_NAME, actions=[ check_webots_ready, - WEBOTS_LAUNCHER_ACTION + launch_webots_controller, ], title="Launch", description="Launch application with Webots launcher." From 5fcf6923a3945c8bb089f1864d7f840d97fa5043 Mon Sep 17 00:00:00 2001 From: jonpg01 Date: Thu, 21 May 2026 14:42:28 +0200 Subject: [PATCH 06/12] feat(APPSlam): add initial implementation of SLAM application --- lib/APPSlam/library.json | 15 + lib/APPSlam/src/App.cpp | 492 ++++++++++++++++++++++++++++ lib/APPSlam/src/App.h | 216 ++++++++++++ lib/APPSlam/src/SerialMuxChannels.h | 255 ++++++++++++++ platformio.ini | 68 ++++ 5 files changed, 1046 insertions(+) create mode 100644 lib/APPSlam/library.json create mode 100644 lib/APPSlam/src/App.cpp create mode 100644 lib/APPSlam/src/App.h create mode 100644 lib/APPSlam/src/SerialMuxChannels.h diff --git a/lib/APPSlam/library.json b/lib/APPSlam/library.json new file mode 100644 index 00000000..6f4bef6a --- /dev/null +++ b/lib/APPSlam/library.json @@ -0,0 +1,15 @@ +{ + "name": "APPSlam", + "version": "0.1.0", + "description": "SLAM application", + "authors": [{ + "name": "Jonas Hochhaus", + "email": "jonas.hochhaus01@gmail.com", + "url": "https://github.com/jonpg01", + "maintainer": true + }], + "license": "MIT", + "dependencies": [], + "frameworks": "*", + "platforms": "*" +} diff --git a/lib/APPSlam/src/App.cpp b/lib/APPSlam/src/App.cpp new file mode 100644 index 00000000..619962b7 --- /dev/null +++ b/lib/APPSlam/src/App.cpp @@ -0,0 +1,492 @@ +/* MIT License + * + * Copyright (c) 2023 - 2026 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @file + * @brief SLAM application + * @author Gabryel Reyes + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "App.h" +#include +#include +#include +#include +#include +#include +#include + +/****************************************************************************** + * Compiler Switches + *****************************************************************************/ + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Configuration of the logging severity if not previously defined. */ +#ifndef CONFIG_LOG_SEVERITY +#define CONFIG_LOG_SEVERITY (Logging::LOG_LEVEL_INFO) +#endif /* CONFIG_LOG_SEVERITY */ + +/** TCP server IP address if not defined by the build configuration. */ +#define APPSLAM_TCP_SERVER_IP "127.0.0.1" + + +/** TCP server port if not defined by the build configuration. */ +#define APPSLAM_TCP_SERVER_PORT 8888U + + +/****************************************************************************** + * Types and classes + *****************************************************************************/ + +/****************************************************************************** + * Prototypes + *****************************************************************************/ + +static void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); +static void App_lineSensorChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); +static void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); + + +/****************************************************************************** + * Local Variables + *****************************************************************************/ + +/** Serial interface baudrate. */ +static const uint32_t SERIAL_BAUDRATE = 115200U; + +/** Serial log sink */ +static LogSinkPrinter gLogSinkSerial("Serial", &Serial); + +/** TCP reconnect interval in milliseconds. */ +static const uint32_t TCP_RECONNECT_INTERVAL = 2000U; + + +/****************************************************************************** + * Public Methods + *****************************************************************************/ + +void App::setup() +{ + bool isSuccessful = false; + SettingsHandler& settings = SettingsHandler::getInstance(); + Board& board = Board::getInstance(); + + Serial.begin(SERIAL_BAUDRATE); + + /* Register serial log sink and select it per default. */ + if (true == Logging::getInstance().registerSink(&gLogSinkSerial)) + { + (void)Logging::getInstance().selectSink(gLogSinkSerial.getName()); + + /* Set severity of logging system. */ + Logging::getInstance().setLogLevel(CONFIG_LOG_SEVERITY); + } + + /* Initialize HAL. */ + if (false == board.init()) + { + LOG_FATAL("HAL init failed."); + } + /* Settings shall be loaded from configuration file. */ + else if (false == settings.loadConfigurationFile(board.getConfigFilePath())) + { + LOG_FATAL("Settings could not be loaded from %s.", board.getConfigFilePath()); + } + else + { + NetworkSettings networkSettings = {settings.getWiFiSSID(), settings.getWiFiPassword(), settings.getRobotName(), + ""}; + + /* If the robot name is empty, use the wifi MAC address as robot name. */ + if (true == settings.getRobotName().isEmpty()) + { + String robotName = WiFi.macAddress(); + + /* Remove MAC separators from robot name. */ + robotName.replace(":", ""); + + settings.setRobotName(robotName); + } + + if (false == board.getNetwork().setConfig(networkSettings)) + { + LOG_FATAL("Network configuration could not be set."); + } + else if (false == setupSerialMuxProtServer()) + { + LOG_FATAL("SerialMuxProt server could not be setup."); + } + else + { + (void)connectToServer(); + m_statusTimer.start(1000U); + isSuccessful = true; + } + } + + if (false == isSuccessful) + { + LOG_FATAL("Initialization failed."); + fatalErrorHandler(); + } +} + +void App::loop() +{ + if (false == m_isFatalError) + { + /* Process Battery, Device and Network. */ + Board::getInstance().process(); + + /* Handle TCP communication. */ + if (true == m_tcpClient.connected()) + { + receivePackets(); + } + else + { + uint32_t now = millis(); + + if ((now - m_lastReconnectAttempt) >= TCP_RECONNECT_INTERVAL) + { + m_lastReconnectAttempt = now; + + LOG_INFO("TCP server disconnected."); + m_tcpClient.stop(); + + if (false == connectToServer()) + { + LOG_WARNING("TCP reconnect attempt failed."); + } + } + } + + /* Process SerialMuxProt. */ + m_smpServer.process(millis()); + + if ((false == m_initialDataSent) && (true == m_smpServer.isSynced())) + { + SettingsHandler& settings = SettingsHandler::getInstance(); + Command cmd = {SMPChannelPayload::CMD_ID_SET_INIT_POS, settings.getInitialXPosition(), + settings.getInitialYPosition(), settings.getInitialHeading()}; + + if (true == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &cmd, sizeof(cmd))) + { + LOG_DEBUG("Initial vehicle data sent."); + m_initialDataSent = true; + } + else + { + LOG_WARNING("Failed to send initial vehicle data."); + } + } + + if ((true == m_statusTimer.isTimeout()) && (true == m_smpServer.isSynced())) + { + Status payload = {SMPChannelPayload::Status::STATUS_FLAG_OK}; + + if (false == m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload))) + { + LOG_WARNING("Failed to send current status to RU."); + } + + m_statusTimer.restart(); + } + } +} + +/****************************************************************************** + * Protected Methods + *****************************************************************************/ + +/****************************************************************************** + * Private Methods + *****************************************************************************/ + +void App::fatalErrorHandler() +{ + if (false == m_isFatalError) + { + /* Turn on Red LED to signal fatal error. */ + Board::getInstance().getRedLed().enable(true); + } + + m_isFatalError = true; +} + +bool App::setupSerialMuxProtServer() +{ + bool isSuccessful = false; + + m_serialMuxProtChannelIdRemoteCtrl = m_smpServer.createChannel(COMMAND_CHANNEL_NAME, COMMAND_CHANNEL_DLC); + m_serialMuxProtChannelIdMotorSpeeds = + m_smpServer.createChannel(MOTOR_SPEED_SETPOINT_CHANNEL_NAME, MOTOR_SPEED_SETPOINT_CHANNEL_DLC); + m_serialMuxProtChannelIdRobotSpeeds = + m_smpServer.createChannel(ROBOT_SPEED_SETPOINT_CHANNEL_NAME, ROBOT_SPEED_SETPOINT_CHANNEL_DLC); + m_serialMuxProtChannelIdStatus = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC); + + m_smpServer.subscribeToChannel(COMMAND_RESPONSE_CHANNEL_NAME, App_cmdRspChannelCallback); + m_smpServer.subscribeToChannel(LINE_SENSOR_CHANNEL_NAME, App_lineSensorChannelCallback); + m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_NAME, App_currentVehicleChannelCallback); + + if ((0U == m_serialMuxProtChannelIdRemoteCtrl) || (0U == m_serialMuxProtChannelIdMotorSpeeds) || + (0U == m_serialMuxProtChannelIdRobotSpeeds) || (0U == m_serialMuxProtChannelIdStatus)) + { + LOG_ERROR("Failed to create SerialMuxProt channels."); + } + else + { + isSuccessful = true; + } + + return isSuccessful; +} + +bool App::connectToServer() +{ + bool isSuccessful = false; + IPAddress serverIp; + + if (false == serverIp.fromString(APPSLAM_TCP_SERVER_IP)) + { + LOG_ERROR("Invalid APPSlam TCP server IP: %s", APPSLAM_TCP_SERVER_IP); + } + else if (true == m_tcpClient.connect(serverIp, APPSLAM_TCP_SERVER_PORT)) + { + LOG_INFO("TCP server connected to %s:%u", APPSLAM_TCP_SERVER_IP, APPSLAM_TCP_SERVER_PORT); + isSuccessful = true; + } + else + { + LOG_WARNING("Failed to connect to TCP server %s:%u", APPSLAM_TCP_SERVER_IP, APPSLAM_TCP_SERVER_PORT); + } + + return isSuccessful; +} + +void App::sendPacket(const String& payload) +{ + if (true == m_tcpClient.connected()) + { + String packet = payload + '\n'; + (void)m_tcpClient.write(reinterpret_cast(packet.c_str()), packet.length()); + + LOG_DEBUG("TX: %s", payload.c_str()); + } +} + +void App::receivePackets() +{ + uint8_t buffer[128U]; + int bytesRead = 0; + + do + { + bytesRead = m_tcpClient.read(buffer, sizeof(buffer)); + + if (bytesRead > 0) + { + for (int i = 0; i < bytesRead; ++i) + { + char c = static_cast(buffer[i]); + + if ('\n' == c) + { + const int newlineIndex = m_tcpRxBuffer.lastIndexOf(String("\n")); + + if (newlineIndex >= 0) + { + m_tcpRxBuffer = m_tcpRxBuffer.substring(0U, static_cast(newlineIndex)); + } + + if (0 != m_tcpRxBuffer.length()) + { + processIncomingLine(m_tcpRxBuffer); + } + + m_tcpRxBuffer = String(); + } + else + { + m_tcpRxBuffer += c; + } + } + } + } while (bytesRead > 0); + + if (bytesRead < 0) + { + /* Error reading from TCP client. Close connection. */ + LOG_WARNING("TCP read error: %d", bytesRead); + m_tcpClient.stop(); + } +} + +void App::processIncomingLine(const String& line) +{ + LOG_DEBUG("RX: %s", line.c_str()); + + /* Handle velocity command via ArduinoJson */ + { + JsonDocument doc; + DeserializationError err = deserializeJson(doc, line.c_str()); + + if (DeserializationError::Ok == err) + { + if ((!doc["linear"].isNull()) && (!doc["angular"].isNull())) + { + int32_t linearCenter = doc["linear"].as(); + int32_t angular = doc["angular"].as(); + + RobotSpeed robotSpeed = {linearCenter, angular}; + LOG_INFO("Received velocity command - Linear: %d mm/s, Angular: %d mrad/s", linearCenter, angular); + if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRobotSpeeds, &robotSpeed, sizeof(robotSpeed))) + { + LOG_WARNING("Failed to send velocity command to RU."); + } + else + { + LOG_DEBUG("Velocity command sent - Linear: %d mm/s, Angular: %d mrad/s", linearCenter, angular); + } + } + else + { + LOG_WARNING("JSON does not contain expected fields 'linear' and 'angular'."); + } + } + else + { + LOG_WARNING("Failed to parse JSON: %s", err.c_str()); + } + } +} + +void App::handleVehicleData(const VehicleData* vehicleData) +{ + if ((nullptr != vehicleData) && (true == m_tcpClient.connected())) + { + /* Format vehicle data as JSON using ArduinoJson and send via TCP */ + JsonDocument doc; + doc["type"] = "vehicle_data"; + doc["t"] = vehicleData->timestamp; + doc["x"] = vehicleData->xPos; + doc["y"] = vehicleData->yPos; + doc["h"] = vehicleData->orientation; + doc["l"] = vehicleData->left; + doc["r"] = vehicleData->right; + doc["c"] = vehicleData->center; + doc["ax"] = vehicleData->accelerationX; + doc["tz"] = vehicleData->turnRateZ; + + String out; + serializeJson(doc, out); + sendPacket(out); + } +} + +/****************************************************************************** + * External Functions + *****************************************************************************/ + +/****************************************************************************** + * Local Functions + *****************************************************************************/ + +/** + * Receives remote control command responses over SerialMuxProt channel. + * + * @param[in] payload Command id + * @param[in] payloadSize Size of command id + * @param[in] userData User data + */ +void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + UTIL_NOT_USED(userData); + if ((nullptr != payload) && (COMMAND_RESPONSE_CHANNEL_DLC == payloadSize)) + { + const CommandResponse* cmdRsp = reinterpret_cast(payload); + LOG_DEBUG("CMD_RSP: ID: 0x%02X , RSP: 0x%02X", cmdRsp->commandId, cmdRsp->responseId); + + if (SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED == cmdRsp->commandId) + { + LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed); + } + } + else + { + LOG_WARNING("CMD_RSP: Invalid payload size. Expected: %u Received: %u", COMMAND_RESPONSE_CHANNEL_DLC, + payloadSize); + } +} + +/** + * Receives line sensor data over SerialMuxProt channel. + * @param[in] payload Line sensor data + * @param[in] payloadSize Size of 5 line sensor data + * @param[in] userData User data + */ +void App_lineSensorChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + UTIL_NOT_USED(payload); + UTIL_NOT_USED(payloadSize); + UTIL_NOT_USED(userData); +} + +/** + * Receives current position, heading and IMU of the robot over SerialMuxProt channel. + * + * @param[in] payload Current vehicle data: Timestamp, two coordinates, one orientation, + * three motor speeds and raw IMU data (X acceleration, Z axis turn rate). + * @param[in] payloadSize The size of the received payload data. + * @param[in] userData Instance of App class. + */ +void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData) +{ + if ((nullptr != payload) && (CURRENT_VEHICLE_DATA_CHANNEL_DLC == payloadSize) && (nullptr != userData)) + { + const VehicleData* currentVehicleData = reinterpret_cast(payload); + App* appInstance = reinterpret_cast(userData); + + LOG_DEBUG("Timestamp: %d X: %d Y: %d Heading: %d Left: %d Right: %d Center: %d AccX: %d TurnZ: %d ", + currentVehicleData->timestamp, currentVehicleData->xPos, currentVehicleData->yPos, + currentVehicleData->orientation, currentVehicleData->left, currentVehicleData->right, + currentVehicleData->center, currentVehicleData->accelerationX, currentVehicleData->turnRateZ); + + /* Forward vehicle data to TCP connection */ + appInstance->handleVehicleData(currentVehicleData); + } + else + { + LOG_WARNING("%s: Invalid payload size. Expected: %u Received: %u", CURRENT_VEHICLE_DATA_CHANNEL_NAME, + CURRENT_VEHICLE_DATA_CHANNEL_DLC, payloadSize); + } +} diff --git a/lib/APPSlam/src/App.h b/lib/APPSlam/src/App.h new file mode 100644 index 00000000..0a259dc2 --- /dev/null +++ b/lib/APPSlam/src/App.h @@ -0,0 +1,216 @@ +/* MIT License + * + * Copyright (c) 2023 - 2026 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @file + * @brief SLAM application + * @author Gabryel Reyes + * + * @addtogroup Application + * + * @{ + */ + +#ifndef APP_H +#define APP_H + +/****************************************************************************** + * Compile Switches + *****************************************************************************/ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include +#include +#include "SerialMuxChannels.h" +#include +#include + + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** The SLAM application. */ +class App +{ +public: + /** + * Construct the SLAM application. + */ + App() : + m_smpServer(Board::getInstance().getRobot().getStream()), + m_serialMuxProtChannelIdRemoteCtrl(0U), + m_serialMuxProtChannelIdMotorSpeeds(0U), + m_serialMuxProtChannelIdRobotSpeeds(0U), + m_serialMuxProtChannelIdStatus(0U), + m_initialDataSent(false), + m_tcpRxBuffer(), + m_lastReconnectAttempt(0U), + m_statusTimer(), + m_isFatalError(false) + { + m_smpServer.setUserData(this); + } + + /** + * Destroy the SLAM application. + */ + ~App() + { + } + + /** + * Setup the application. + */ + void setup(); + + /** + * Process the application periodically. + */ + void loop(); + + /** + * Handle vehicle data received from SerialMuxProt and forward to TCP. + * + * @param[in] vehicleData Pointer to vehicle data. + */ + void handleVehicleData(const VehicleData* vehicleData); + +private: + /** SerialMuxProt Channel ID for sending remote control commands. */ + uint8_t m_serialMuxProtChannelIdRemoteCtrl; + + /** SerialMuxProt Channel ID for sending motor speeds. */ + uint8_t m_serialMuxProtChannelIdMotorSpeeds; + + /** SerialMuxProt Channel ID for sending robot speed setpoints (linear + angular). */ + uint8_t m_serialMuxProtChannelIdRobotSpeeds; + + /** SerialMuxProt Channel ID for sending system status. */ + uint8_t m_serialMuxProtChannelIdStatus; + + /** TCP client for communication with the companion service. */ + WiFiClient m_tcpClient; + + /** + * SerialMuxProt Server Instance + * + * @tparam tMaxChannels set to MAX_CHANNELS, defined in SerialMuxChannels.h. + */ + SMPServer m_smpServer; + + /** + * Flag for setting initial data through SMP. + */ + bool m_initialDataSent; + + /** TCP receive buffer. */ + String m_tcpRxBuffer; + + /** Last reconnect attempt time. */ + uint32_t m_lastReconnectAttempt; + + /** + * Timer for sending system status to RU. + */ + SimpleTimer m_statusTimer; + + /** + * Is fatal error happened? + */ + bool m_isFatalError; + +private: + /** + * Handler of fatal errors in the Application. + */ + void fatalErrorHandler(); + + /** + * Setup the SerialMuxProt Server. + * + * @returns true if successful, otherwise false. + */ + bool setupSerialMuxProtServer(); + + /** + * Connect to the TCP server. + * + * @returns true if successful, otherwise false. + */ + bool connectToServer(); + + /** + * Send a TCP packet. + * + * @param[in] payload Payload to send. + */ + void sendPacket(const String& payload); + + /** + * Receive TCP packets. + */ + void receivePackets(); + + /** + * Process one incoming TCP line. + * + * @param[in] line Incoming line. + */ + void processIncomingLine(const String& line); + + /** + * Copy construction of an instance. + * Not allowed. + * + * @param[in] app Source instance. + */ + App(const App& app); + + /** + * Assignment of an instance. + * Not allowed. + * + * @param[in] app Source instance. + * + * @returns Reference to App instance. + */ + App& operator=(const App& app); +}; + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* APP_H */ +/** @} */ diff --git a/lib/APPSlam/src/SerialMuxChannels.h b/lib/APPSlam/src/SerialMuxChannels.h new file mode 100644 index 00000000..ab647243 --- /dev/null +++ b/lib/APPSlam/src/SerialMuxChannels.h @@ -0,0 +1,255 @@ +/* MIT License + * + * Copyright (c) 2023 - 2026 Andreas Merkle + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +/******************************************************************************* + DESCRIPTION +*******************************************************************************/ +/** + * @file + * @brief Channel structure definition for the SerialMuxProt. + * @author Gabryel Reyes + * + * @addtogroup Application + * + * @{ + */ + +#ifndef SERIAL_MUX_CHANNELS_H_ +#define SERIAL_MUX_CHANNELS_H_ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include +#include + +/****************************************************************************** + * Macros + *****************************************************************************/ + +/** Maximum number of SerialMuxProt Channels. */ +#define MAX_CHANNELS (10U) + +/** Name of Channel to send Commands to. */ +#define COMMAND_CHANNEL_NAME "CMD" + +/** DLC of Command Channel. */ +#define COMMAND_CHANNEL_DLC (sizeof(Command)) + +/** Name of Channel to receive Command Responses from. */ +#define COMMAND_RESPONSE_CHANNEL_NAME "CMD_RSP" + +/** DLC of Command Response Channel. */ +#define COMMAND_RESPONSE_CHANNEL_DLC (sizeof(CommandResponse)) + +/** Name of Channel to send Motor Speed Setpoints to. */ +#define MOTOR_SPEED_SETPOINT_CHANNEL_NAME "MOTOR_SET" + +/** DLC of Motor Speed Setpoint Channel */ +#define MOTOR_SPEED_SETPOINT_CHANNEL_DLC (sizeof(MotorSpeed)) + +/** Name of the Channel to send Robot Speed Setpoints to. */ +#define ROBOT_SPEED_SETPOINT_CHANNEL_NAME "ROBOT_SET" + +/** DLC of Robot Speed Setpoint Channel */ +#define ROBOT_SPEED_SETPOINT_CHANNEL_DLC (sizeof(RobotSpeed)) + +/** Name of Channel to receive Current Vehicle Data from. */ +#define CURRENT_VEHICLE_DATA_CHANNEL_NAME "CURR_DATA" + +/** DLC of Current Vehicle Data Channel */ +#define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData)) + +/** Name of Channel to send system status to. */ +#define STATUS_CHANNEL_NAME "STATUS" + +/** DLC of Status Channel */ +#define STATUS_CHANNEL_DLC (sizeof(Status)) + +/** Name of the Channel to receive Line Sensor Data from. */ +#define LINE_SENSOR_CHANNEL_NAME "LINE_SENS" + +/** DLC of Line Sensor Channel */ +#define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData)) + +/****************************************************************************** + * Types and Classes + *****************************************************************************/ + +/** SerialMuxProt Server with fixed template argument. */ +typedef SerialMuxProtServer SMPServer; + +/** Channel payload constants. */ +namespace SMPChannelPayload +{ + /** Remote control commands. */ + typedef enum : uint8_t + { + CMD_ID_IDLE = 0, /**< Nothing to do. */ + CMD_ID_START_LINE_SENSOR_CALIB, /**< Start line sensor calibration. */ + CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */ + CMD_ID_REINIT_BOARD, /**< Re-initialize the board. Required for webots simulation. */ + CMD_ID_GET_MAX_SPEED, /**< Get maximum speed. */ + CMD_ID_START_DRIVING, /**< Start driving. */ + CMD_ID_SET_INIT_POS /**< Set initial position. */ + + } CmdId; /**< Command ID */ + + /** Remote control command responses. */ + typedef enum : uint8_t + { + RSP_ID_OK = 0, /**< Command successful executed. */ + RSP_ID_PENDING, /**< Command is pending. */ + RSP_ID_ERROR /**< Command failed. */ + + } RspId; /**< Response ID */ + + /** Status flags. */ + typedef enum : uint8_t + { + STATUS_FLAG_OK = 0, /**< Everything is fine. */ + STATUS_FLAG_ERROR /**< Something is wrong. */ + + } Status; /**< Status flag */ + + /** + * Range in which a detected object may be. + * Equivalent to the brightness levels. + * Values estimated from user's guide. + */ + typedef enum : uint8_t + { + RANGE_NO_OBJECT = 0U, /**< No object detected */ + RANGE_25_30, /**< Object detected in range 25 to 30 cm */ + RANGE_20_25, /**< Object detected in range 20 to 25 cm */ + RANGE_15_20, /**< Object detected in range 15 to 20 cm */ + RANGE_10_15, /**< Object detected in range 10 to 15 cm */ + RANGE_5_10, /**< Object detected in range 5 to 10 cm */ + RANGE_0_5 /**< Object detected in range 0 to 5 cm */ + + } Range; /**< Proximity Sensor Ranges */ + +} /* namespace SMPChannelPayload */ + +/** Struct of the "Command" channel payload. */ +typedef struct _Command +{ + SMPChannelPayload::CmdId commandId; /**< Command ID */ + + /** Command payload. */ + union + { + /** Init data command payload. */ + struct + { + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ + }; + }; + +} __attribute__((packed)) Command; + +static_assert(sizeof(Command) <= MAX_DATA_LEN, + "Command struct size must be less than or equal to MAX_DATA_LEN to fit in the SerialMuxProt frame."); + +/** Struct of the "Command Response" channel payload. */ +typedef struct _CommandResponse +{ + SMPChannelPayload::CmdId commandId; /**< Command ID */ + SMPChannelPayload::RspId responseId; /**< Response to the command */ + + /** Response Payload. */ + union + { + int32_t maxMotorSpeed; /**< Max speed [mm/s]. */ + }; +} __attribute__((packed)) CommandResponse; + +static_assert( + sizeof(CommandResponse) <= MAX_DATA_LEN, + "CommandResponse struct size must be less than or equal to MAX_DATA_LEN to fit in the SerialMuxProt frame."); + +/** Struct of the "Motor Speed Setpoints" channel payload. */ +typedef struct _MotorSpeed +{ + int32_t left; /**< Left motor speed [mm/s] */ + int32_t right; /**< Right motor speed [mm/s] */ +} __attribute__((packed)) MotorSpeed; + +static_assert(sizeof(MotorSpeed) <= MAX_DATA_LEN, + "MotorSpeed struct size must be less than or equal to MAX_DATA_LEN to fit in the SerialMuxProt frame."); + +/** Struct of the "Robot Speed Setpoints" channel payload. */ +typedef struct _RobotSpeed +{ + int32_t linearCenter; /**< Linear speed of the vehicle center. [mm/s] */ + int32_t angular; /**< Angular speed. [mrad/s] */ +} __attribute__((packed)) RobotSpeed; + +static_assert(sizeof(RobotSpeed) <= MAX_DATA_LEN, + "RobotSpeed struct size must be less than or equal to MAX_DATA_LEN to fit in the SerialMuxProt frame."); + +/** Struct of the "Current Vehicle Data" channel payload. */ +typedef struct _VehicleData +{ + uint32_t timestamp; /**< Timestamp [ms]. */ + int32_t xPos; /**< X position [mm]. */ + int32_t yPos; /**< Y position [mm]. */ + int32_t orientation; /**< Orientation [mrad]. */ + int32_t left; /**< Left motor speed [mm/s]. */ + int32_t right; /**< Right motor speed [mm/s]. */ + int32_t center; /**< Center speed [mm/s]. */ + SMPChannelPayload::Range proximity; /**< Range at which object is found [range]. */ + int16_t accelerationX; /**< Raw acceleration in X [digit]. */ + int16_t turnRateZ; /**< Raw turn rate around Z [digit]. */ +} __attribute__((packed)) VehicleData; + +static_assert(sizeof(VehicleData) <= MAX_DATA_LEN, + "VehicleData struct size must be less than or equal to MAX_DATA_LEN to fit in the SerialMuxProt frame."); + +/** Struct of the "Status" channel payload. */ +typedef struct _Status +{ + SMPChannelPayload::Status status; /**< Status */ +} __attribute__((packed)) Status; + +static_assert(sizeof(Status) <= MAX_DATA_LEN, + "Status struct size must be less than or equal to MAX_DATA_LEN to fit in the SerialMuxProt frame."); + +/** Struct of the "Line Sensor" channel payload. */ +typedef struct _LineSensorData +{ + uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */ +} __attribute__((packed)) LineSensorData; + +static_assert( + sizeof(LineSensorData) <= MAX_DATA_LEN, + "LineSensorData struct size must be less than or equal to MAX_DATA_LEN to fit in the SerialMuxProt frame."); + +/****************************************************************************** + * Functions + *****************************************************************************/ + +#endif /* SERIAL_MUX_CHANNELS_H_ */ +/** @} */ diff --git a/platformio.ini b/platformio.ini index 7d71fe9d..15dafcad 100644 --- a/platformio.ini +++ b/platformio.ini @@ -152,6 +152,7 @@ lib_ignore = APPLineFollower APPRemoteControl APPSensorFusion + APPSlam APPTest APPTurtle @@ -173,6 +174,7 @@ lib_ignore = APPLineFollower APPRemoteControl APPSensorFusion + APPSlam APPTest APPTurtle @@ -193,6 +195,7 @@ lib_ignore = APPConvoyFollower APPRemoteControl APPSensorFusion + APPSlam APPTest APPTurtle @@ -213,6 +216,28 @@ lib_ignore = APPConvoyFollower APPLineFollower APPSensorFusion + APPSlam + APPTest + APPTurtle + +; ***************************************************************************** +; SLAM application +; ***************************************************************************** +[app:Slam] +build_flags = + -D MAX_DATA_LEN=36U ; Overwrite SerialMuxProt max. data length +lib_deps = + APPSlam + Service + Utilities + gabryelreyes/SerialMuxProt @ ^2.4.0 + bblanchon/ArduinoJson @ ^7.4.2 +lib_ignore = + APPConvoyLeader + APPConvoyFollower + APPLineFollower + APPRemoteControl + APPSensorFusion APPTest APPTurtle @@ -231,6 +256,7 @@ lib_ignore = APPLineFollower APPRemoteControl APPSensorFusion + APPSlam APPTurtle ; ***************************************************************************** @@ -249,6 +275,7 @@ lib_ignore = APPConvoyFollower APPLineFollower APPRemoteControl + APPSlam APPTest APPTurtle @@ -270,6 +297,7 @@ lib_ignore = APPLineFollower APPRemoteControl APPSensorFusion + APPSlam APPTest ; ***************************************************************************** @@ -432,6 +460,46 @@ check_src_filters = +<*> - +; ***************************************************************************** +; SLAM application on simulation +; ***************************************************************************** +[env:SlamSim] +extends = target:Sim, app:Slam, static_check_configuration +build_flags = + ${target:Sim.build_flags} + ${app:Slam.build_flags} +lib_deps = + ${target:Sim.lib_deps} + ${app:Slam.lib_deps} +lib_ignore = + ${target:Sim.lib_ignore} + ${app:Slam.lib_ignore} +extra_scripts = + ${target:Sim.extra_scripts} +check_src_filters = + +<*> + - + +; ***************************************************************************** +; SLAM application on target +; ***************************************************************************** +[env:SlamTarget] +extends = target:esp32, app:Slam, static_check_configuration +build_flags = + ${target:esp32.build_flags} + ${app:Slam.build_flags} +lib_deps = + ${target:esp32.lib_deps} + ${app:Slam.lib_deps} +lib_ignore = + ${target:esp32.lib_ignore} + ${app:Slam.lib_ignore} +extra_scripts = + ${target:esp32.extra_scripts} +check_src_filters = + +<*> + - + ; ***************************************************************************** ; Sensor Fusion application on simulation ; ***************************************************************************** From 5f48a25408eaf1c46764b21e39c45ef48f408c57 Mon Sep 17 00:00:00 2001 From: jonpg01 Date: Wed, 24 Jun 2026 22:49:32 +0200 Subject: [PATCH 07/12] Fix indentation and white space --- lib/APPSlam/src/App.cpp | 30 +++++++++++++----------------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/lib/APPSlam/src/App.cpp b/lib/APPSlam/src/App.cpp index 619962b7..decbc222 100644 --- a/lib/APPSlam/src/App.cpp +++ b/lib/APPSlam/src/App.cpp @@ -58,11 +58,9 @@ /** TCP server IP address if not defined by the build configuration. */ #define APPSLAM_TCP_SERVER_IP "127.0.0.1" - /** TCP server port if not defined by the build configuration. */ #define APPSLAM_TCP_SERVER_PORT 8888U - /****************************************************************************** * Types and classes *****************************************************************************/ @@ -75,7 +73,6 @@ static void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payl static void App_lineSensorChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); static void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData); - /****************************************************************************** * Local Variables *****************************************************************************/ @@ -89,7 +86,6 @@ static LogSinkPrinter gLogSinkSerial("Serial", &Serial); /** TCP reconnect interval in milliseconds. */ static const uint32_t TCP_RECONNECT_INTERVAL = 2000U; - /****************************************************************************** * Public Methods *****************************************************************************/ @@ -273,7 +269,7 @@ bool App::setupSerialMuxProtServer() bool App::connectToServer() { - bool isSuccessful = false; + bool isSuccessful = false; IPAddress serverIp; if (false == serverIp.fromString(APPSLAM_TCP_SERVER_IP)) @@ -307,7 +303,7 @@ void App::sendPacket(const String& payload) void App::receivePackets() { uint8_t buffer[128U]; - int bytesRead = 0; + int bytesRead = 0; do { @@ -357,7 +353,7 @@ void App::processIncomingLine(const String& line) /* Handle velocity command via ArduinoJson */ { - JsonDocument doc; + JsonDocument doc; DeserializationError err = deserializeJson(doc, line.c_str()); if (DeserializationError::Ok == err) @@ -365,7 +361,7 @@ void App::processIncomingLine(const String& line) if ((!doc["linear"].isNull()) && (!doc["angular"].isNull())) { int32_t linearCenter = doc["linear"].as(); - int32_t angular = doc["angular"].as(); + int32_t angular = doc["angular"].as(); RobotSpeed robotSpeed = {linearCenter, angular}; LOG_INFO("Received velocity command - Linear: %d mm/s, Angular: %d mrad/s", linearCenter, angular); @@ -397,15 +393,15 @@ void App::handleVehicleData(const VehicleData* vehicleData) /* Format vehicle data as JSON using ArduinoJson and send via TCP */ JsonDocument doc; doc["type"] = "vehicle_data"; - doc["t"] = vehicleData->timestamp; - doc["x"] = vehicleData->xPos; - doc["y"] = vehicleData->yPos; - doc["h"] = vehicleData->orientation; - doc["l"] = vehicleData->left; - doc["r"] = vehicleData->right; - doc["c"] = vehicleData->center; - doc["ax"] = vehicleData->accelerationX; - doc["tz"] = vehicleData->turnRateZ; + doc["t"] = vehicleData->timestamp; + doc["x"] = vehicleData->xPos; + doc["y"] = vehicleData->yPos; + doc["h"] = vehicleData->orientation; + doc["l"] = vehicleData->left; + doc["r"] = vehicleData->right; + doc["c"] = vehicleData->center; + doc["ax"] = vehicleData->accelerationX; + doc["tz"] = vehicleData->turnRateZ; String out; serializeJson(doc, out); From e3bf8dbf785d301e459f4c82036e48c40e8aeb95 Mon Sep 17 00:00:00 2001 From: jonpg01 Date: Thu, 2 Jul 2026 10:08:42 +0200 Subject: [PATCH 08/12] Fixed review findings --- lib/APPSlam/src/App.cpp | 97 ++++++++++++++++++++--------- lib/APPSlam/src/App.h | 31 ++++----- lib/APPSlam/src/SerialMuxChannels.h | 2 +- 3 files changed, 84 insertions(+), 46 deletions(-) diff --git a/lib/APPSlam/src/App.cpp b/lib/APPSlam/src/App.cpp index decbc222..f3cb55b3 100644 --- a/lib/APPSlam/src/App.cpp +++ b/lib/APPSlam/src/App.cpp @@ -27,7 +27,7 @@ /** * @file * @brief SLAM application - * @author Gabryel Reyes + * @author Jonas Hochhaus */ /****************************************************************************** @@ -38,9 +38,8 @@ #include #include #include -#include -#include #include +#include /****************************************************************************** * Compiler Switches @@ -55,11 +54,15 @@ #define CONFIG_LOG_SEVERITY (Logging::LOG_LEVEL_INFO) #endif /* CONFIG_LOG_SEVERITY */ -/** TCP server IP address if not defined by the build configuration. */ +/** ROS2 server IP address. */ +#ifndef APPSLAM_TCP_SERVER_IP #define APPSLAM_TCP_SERVER_IP "127.0.0.1" +#endif /* APPSLAM_TCP_SERVER_IP */ -/** TCP server port if not defined by the build configuration. */ +/** ROS2 server port. */ +#ifndef APPSLAM_TCP_SERVER_PORT #define APPSLAM_TCP_SERVER_PORT 8888U +#endif /* APPSLAM_TCP_SERVER_PORT */ /****************************************************************************** * Types and classes @@ -84,7 +87,16 @@ static const uint32_t SERIAL_BAUDRATE = 115200U; static LogSinkPrinter gLogSinkSerial("Serial", &Serial); /** TCP reconnect interval in milliseconds. */ -static const uint32_t TCP_RECONNECT_INTERVAL = 2000U; +static const uint32_t TCP_RECONNECT_INTERVAL_MS = 2000U; + +/** JSON key for linear velocity. */ +static const char* JSON_KEY_LINEAR = "linear"; + +/** JSON key for angular velocity. */ +static const char* JSON_KEY_ANGULAR = "angular"; + +/** Maximum length of a TCP receive line in characters. */ +static const size_t MAX_LINE_LEN = 512U; /****************************************************************************** * Public Methods @@ -143,7 +155,10 @@ void App::setup() } else { - (void)connectToServer(); + if (false == connectToROS2Server()) + { + LOG_WARNING("Initial ROS2 server connection failed."); + } m_statusTimer.start(1000U); isSuccessful = true; } @@ -172,16 +187,16 @@ void App::loop() { uint32_t now = millis(); - if ((now - m_lastReconnectAttempt) >= TCP_RECONNECT_INTERVAL) + if ((now - m_lastReconnectAttemptTimeMs) >= TCP_RECONNECT_INTERVAL_MS) { - m_lastReconnectAttempt = now; + m_lastReconnectAttemptTimeMs = now; - LOG_INFO("TCP server disconnected."); + LOG_INFO("ROS2 server disconnected. Attempting reconnect..."); m_tcpClient.stop(); - if (false == connectToServer()) + if (false == connectToROS2Server()) { - LOG_WARNING("TCP reconnect attempt failed."); + LOG_WARNING("ROS2 server reconnect attempt failed."); } } } @@ -267,23 +282,24 @@ bool App::setupSerialMuxProtServer() return isSuccessful; } -bool App::connectToServer() +bool App::connectToROS2Server() { + bool isSuccessful = false; bool isSuccessful = false; IPAddress serverIp; if (false == serverIp.fromString(APPSLAM_TCP_SERVER_IP)) { - LOG_ERROR("Invalid APPSlam TCP server IP: %s", APPSLAM_TCP_SERVER_IP); + LOG_ERROR("Invalid APPSlam ROS2 server IP: %s", APPSLAM_TCP_SERVER_IP); } else if (true == m_tcpClient.connect(serverIp, APPSLAM_TCP_SERVER_PORT)) { - LOG_INFO("TCP server connected to %s:%u", APPSLAM_TCP_SERVER_IP, APPSLAM_TCP_SERVER_PORT); + LOG_INFO("ROS2 server connected at %s:%u", APPSLAM_TCP_SERVER_IP, APPSLAM_TCP_SERVER_PORT); isSuccessful = true; } else { - LOG_WARNING("Failed to connect to TCP server %s:%u", APPSLAM_TCP_SERVER_IP, APPSLAM_TCP_SERVER_PORT); + LOG_WARNING("Failed to connect to ROS2 server %s:%u", APPSLAM_TCP_SERVER_IP, APPSLAM_TCP_SERVER_PORT); } return isSuccessful; @@ -293,10 +309,21 @@ void App::sendPacket(const String& payload) { if (true == m_tcpClient.connected()) { - String packet = payload + '\n'; - (void)m_tcpClient.write(reinterpret_cast(packet.c_str()), packet.length()); + String packet = payload + '\n'; + size_t bytesWritten = m_tcpClient.write(reinterpret_cast(packet.c_str()), packet.length()); - LOG_DEBUG("TX: %s", payload.c_str()); + if (bytesWritten != packet.length()) + { + LOG_WARNING("TCP write incomplete: wrote %u of %u bytes.", bytesWritten, packet.length()); + } + else + { + LOG_DEBUG("TX: %s", payload.c_str()); + } + } + else + { + LOG_WARNING("Attempted to send packet while not connected to ROS2 server."); } } @@ -304,6 +331,7 @@ void App::receivePackets() { uint8_t buffer[128U]; int bytesRead = 0; + int bytesRead = 0; do { @@ -317,13 +345,6 @@ void App::receivePackets() if ('\n' == c) { - const int newlineIndex = m_tcpRxBuffer.lastIndexOf(String("\n")); - - if (newlineIndex >= 0) - { - m_tcpRxBuffer = m_tcpRxBuffer.substring(0U, static_cast(newlineIndex)); - } - if (0 != m_tcpRxBuffer.length()) { processIncomingLine(m_tcpRxBuffer); @@ -334,6 +355,12 @@ void App::receivePackets() else { m_tcpRxBuffer += c; + + if (m_tcpRxBuffer.length() >= MAX_LINE_LEN) + { + LOG_WARNING("TCP line too long, discarding."); + m_tcpRxBuffer = String(); + } } } } @@ -353,15 +380,16 @@ void App::processIncomingLine(const String& line) /* Handle velocity command via ArduinoJson */ { + JsonDocument doc; JsonDocument doc; DeserializationError err = deserializeJson(doc, line.c_str()); if (DeserializationError::Ok == err) { - if ((!doc["linear"].isNull()) && (!doc["angular"].isNull())) + if ((!doc[JSON_KEY_LINEAR].isNull()) && (!doc[JSON_KEY_ANGULAR].isNull())) { - int32_t linearCenter = doc["linear"].as(); - int32_t angular = doc["angular"].as(); + int32_t linearCenter = doc[JSON_KEY_LINEAR].as(); + int32_t angular = doc[JSON_KEY_ANGULAR].as(); RobotSpeed robotSpeed = {linearCenter, angular}; LOG_INFO("Received velocity command - Linear: %d mm/s, Angular: %d mrad/s", linearCenter, angular); @@ -371,7 +399,7 @@ void App::processIncomingLine(const String& line) } else { - LOG_DEBUG("Velocity command sent - Linear: %d mm/s, Angular: %d mrad/s", linearCenter, angular); + LOG_DEBUG("Velocity command sent."); } } else @@ -404,8 +432,15 @@ void App::handleVehicleData(const VehicleData* vehicleData) doc["tz"] = vehicleData->turnRateZ; String out; - serializeJson(doc, out); + if (0U < serializeJson(doc, out)) + { sendPacket(out); + } + else + { + LOG_WARNING("Failed to serialize vehicle data."); + } + } } diff --git a/lib/APPSlam/src/App.h b/lib/APPSlam/src/App.h index 0a259dc2..22633ca9 100644 --- a/lib/APPSlam/src/App.h +++ b/lib/APPSlam/src/App.h @@ -27,7 +27,7 @@ /** * @file * @brief SLAM application - * @author Gabryel Reyes + * @author Jonas Hochhaus * * @addtogroup Application * @@ -75,7 +75,7 @@ class App m_serialMuxProtChannelIdStatus(0U), m_initialDataSent(false), m_tcpRxBuffer(), - m_lastReconnectAttempt(0U), + m_lastReconnectAttemptTimeMs(0U), m_statusTimer(), m_isFatalError(false) { @@ -100,7 +100,7 @@ class App void loop(); /** - * Handle vehicle data received from SerialMuxProt and forward to TCP. + * Handle vehicle data received from SerialMuxProt and forward to ROS2 server. * * @param[in] vehicleData Pointer to vehicle data. */ @@ -119,7 +119,7 @@ class App /** SerialMuxProt Channel ID for sending system status. */ uint8_t m_serialMuxProtChannelIdStatus; - /** TCP client for communication with the companion service. */ + /** TCP client for communication with the ROS2 server. */ WiFiClient m_tcpClient; /** @@ -130,15 +130,15 @@ class App SMPServer m_smpServer; /** - * Flag for setting initial data through SMP. - */ + * Flag for setting initial data through SMP. + */ bool m_initialDataSent; /** TCP receive buffer. */ String m_tcpRxBuffer; - /** Last reconnect attempt time. */ - uint32_t m_lastReconnectAttempt; + /** Timestamp of the last TCP reconnect attempt [ms]. */ + uint32_t m_lastReconnectAttemptTimeMs; /** * Timer for sending system status to RU. @@ -164,28 +164,31 @@ class App bool setupSerialMuxProtServer(); /** - * Connect to the TCP server. + * Connect to the ROS2 server via TCP. * * @returns true if successful, otherwise false. */ - bool connectToServer(); + bool connectToROS2Server(); /** - * Send a TCP packet. + * Send a newline-terminated TCP packet to the ROS2 server. * * @param[in] payload Payload to send. */ void sendPacket(const String& payload); /** - * Receive TCP packets. + * Receive and buffer TCP packets from the ROS2 server. + * Processes complete newline-terminated lines. */ void receivePackets(); /** - * Process one incoming TCP line. + * Parse and process one incoming JSON line from the ROS2 server. + * Expects a JSON object with "linear" (mm/s) and "angular" (mrad/s) velocity fields. + * Logs a warning if parsing fails or the expected fields are missing. * - * @param[in] line Incoming line. + * @param[in] line JSON string to parse and process. */ void processIncomingLine(const String& line); diff --git a/lib/APPSlam/src/SerialMuxChannels.h b/lib/APPSlam/src/SerialMuxChannels.h index ab647243..6d4692ff 100644 --- a/lib/APPSlam/src/SerialMuxChannels.h +++ b/lib/APPSlam/src/SerialMuxChannels.h @@ -27,7 +27,7 @@ /** * @file * @brief Channel structure definition for the SerialMuxProt. - * @author Gabryel Reyes + * @author Jonas Hochhaus * * @addtogroup Application * From 2f662f9f37938b698ed7ccd63dae8656dc1c3271 Mon Sep 17 00:00:00 2001 From: jonpg01 Date: Thu, 2 Jul 2026 10:19:23 +0200 Subject: [PATCH 09/12] Fix indentation --- lib/APPSlam/src/App.cpp | 2 +- lib/APPSlam/src/App.h | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/lib/APPSlam/src/App.cpp b/lib/APPSlam/src/App.cpp index f3cb55b3..260b1346 100644 --- a/lib/APPSlam/src/App.cpp +++ b/lib/APPSlam/src/App.cpp @@ -434,7 +434,7 @@ void App::handleVehicleData(const VehicleData* vehicleData) String out; if (0U < serializeJson(doc, out)) { - sendPacket(out); + sendPacket(out); } else { diff --git a/lib/APPSlam/src/App.h b/lib/APPSlam/src/App.h index 22633ca9..c8dc24d0 100644 --- a/lib/APPSlam/src/App.h +++ b/lib/APPSlam/src/App.h @@ -51,7 +51,6 @@ #include #include - /****************************************************************************** * Macros *****************************************************************************/ From 5350fac31c42cc6fe7f6384a7ec3d91ee76d3366 Mon Sep 17 00:00:00 2001 From: jonpg01 Date: Thu, 2 Jul 2026 12:16:50 +0200 Subject: [PATCH 10/12] Remove duplicate variables and code cleanup --- lib/APPSlam/src/App.cpp | 12 +++++------- lib/APPSlam/src/App.h | 2 +- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/lib/APPSlam/src/App.cpp b/lib/APPSlam/src/App.cpp index 260b1346..036153dd 100644 --- a/lib/APPSlam/src/App.cpp +++ b/lib/APPSlam/src/App.cpp @@ -54,12 +54,12 @@ #define CONFIG_LOG_SEVERITY (Logging::LOG_LEVEL_INFO) #endif /* CONFIG_LOG_SEVERITY */ -/** ROS2 server IP address. */ +/** ROS2 server IP address if not defined by the build configuration */ #ifndef APPSLAM_TCP_SERVER_IP #define APPSLAM_TCP_SERVER_IP "127.0.0.1" #endif /* APPSLAM_TCP_SERVER_IP */ -/** ROS2 server port. */ +/** ROS2 server port if not defined by the build configuration */ #ifndef APPSLAM_TCP_SERVER_PORT #define APPSLAM_TCP_SERVER_PORT 8888U #endif /* APPSLAM_TCP_SERVER_PORT */ @@ -157,8 +157,9 @@ void App::setup() { if (false == connectToROS2Server()) { - LOG_WARNING("Initial ROS2 server connection failed."); + LOG_WARNING("Initial ROS2 server connection failed. Will retry in loop()."); } + m_statusTimer.start(1000U); isSuccessful = true; } @@ -284,7 +285,6 @@ bool App::setupSerialMuxProtServer() bool App::connectToROS2Server() { - bool isSuccessful = false; bool isSuccessful = false; IPAddress serverIp; @@ -331,7 +331,6 @@ void App::receivePackets() { uint8_t buffer[128U]; int bytesRead = 0; - int bytesRead = 0; do { @@ -380,7 +379,6 @@ void App::processIncomingLine(const String& line) /* Handle velocity command via ArduinoJson */ { - JsonDocument doc; JsonDocument doc; DeserializationError err = deserializeJson(doc, line.c_str()); @@ -440,7 +438,7 @@ void App::handleVehicleData(const VehicleData* vehicleData) { LOG_WARNING("Failed to serialize vehicle data."); } - + } } diff --git a/lib/APPSlam/src/App.h b/lib/APPSlam/src/App.h index c8dc24d0..d5f0eb57 100644 --- a/lib/APPSlam/src/App.h +++ b/lib/APPSlam/src/App.h @@ -99,7 +99,7 @@ class App void loop(); /** - * Handle vehicle data received from SerialMuxProt and forward to ROS2 server. + * Handles vehicle data received from SerialMuxProt and forwards it to the ROS2 server. * * @param[in] vehicleData Pointer to vehicle data. */ From 2c8620b818e2a6ddf288f7859e20be573de9530f Mon Sep 17 00:00:00 2001 From: jonpg01 Date: Mon, 6 Jul 2026 13:00:02 +0200 Subject: [PATCH 11/12] fix: remove unnecessary whitespace and improve documentation comments in App class --- lib/APPSlam/src/App.cpp | 1 - lib/APPSlam/src/App.h | 28 +++++++++++++--------------- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/lib/APPSlam/src/App.cpp b/lib/APPSlam/src/App.cpp index 036153dd..44685fe4 100644 --- a/lib/APPSlam/src/App.cpp +++ b/lib/APPSlam/src/App.cpp @@ -438,7 +438,6 @@ void App::handleVehicleData(const VehicleData* vehicleData) { LOG_WARNING("Failed to serialize vehicle data."); } - } } diff --git a/lib/APPSlam/src/App.h b/lib/APPSlam/src/App.h index d5f0eb57..ba1981e1 100644 --- a/lib/APPSlam/src/App.h +++ b/lib/APPSlam/src/App.h @@ -64,7 +64,7 @@ class App { public: /** - * Construct the SLAM application. + * Constructs the SLAM application. */ App() : m_smpServer(Board::getInstance().getRobot().getStream()), @@ -82,19 +82,19 @@ class App } /** - * Destroy the SLAM application. + * Destroys the SLAM application. */ ~App() { } /** - * Setup the application. + * Sets up the application. */ void setup(); /** - * Process the application periodically. + * Processes the application periodically. */ void loop(); @@ -151,39 +151,39 @@ class App private: /** - * Handler of fatal errors in the Application. + * Handles fatal errors in the application. */ void fatalErrorHandler(); /** - * Setup the SerialMuxProt Server. + * Sets up the SerialMuxProt Server. * * @returns true if successful, otherwise false. */ bool setupSerialMuxProtServer(); /** - * Connect to the ROS2 server via TCP. + * Connects to the ROS2 server via TCP. * * @returns true if successful, otherwise false. */ bool connectToROS2Server(); /** - * Send a newline-terminated TCP packet to the ROS2 server. + * Sends a newline-terminated TCP packet to the ROS2 server. * * @param[in] payload Payload to send. */ void sendPacket(const String& payload); /** - * Receive and buffer TCP packets from the ROS2 server. - * Processes complete newline-terminated lines. + * Receives and buffers TCP packets from the ROS2 server. + * Processes complete newline-terminated lines. */ void receivePackets(); /** - * Parse and process one incoming JSON line from the ROS2 server. + * Parses and processes one incoming JSON line from the ROS2 server. * Expects a JSON object with "linear" (mm/s) and "angular" (mrad/s) velocity fields. * Logs a warning if parsing fails or the expected fields are missing. * @@ -192,16 +192,14 @@ class App void processIncomingLine(const String& line); /** - * Copy construction of an instance. - * Not allowed. + * Prohibits copy construction of an instance. * * @param[in] app Source instance. */ App(const App& app); /** - * Assignment of an instance. - * Not allowed. + * Prohibits assignment of an instance. * * @param[in] app Source instance. * From 69b4a66a3b299534c479242f1e0b984891aa6e76 Mon Sep 17 00:00:00 2001 From: jonpg01 Date: Mon, 6 Jul 2026 15:05:38 +0200 Subject: [PATCH 12/12] fix: correct indentation in documentation comments in App class --- lib/APPSlam/src/App.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/lib/APPSlam/src/App.h b/lib/APPSlam/src/App.h index ba1981e1..0a554e2c 100644 --- a/lib/APPSlam/src/App.h +++ b/lib/APPSlam/src/App.h @@ -64,7 +64,7 @@ class App { public: /** - * Constructs the SLAM application. + * Constructs the SLAM application. */ App() : m_smpServer(Board::getInstance().getRobot().getStream()), @@ -82,19 +82,19 @@ class App } /** - * Destroys the SLAM application. + * Destroys the SLAM application. */ ~App() { } /** - * Sets up the application. + * Sets up the application. */ void setup(); /** - * Processes the application periodically. + * Processes the application periodically. */ void loop(); @@ -151,34 +151,34 @@ class App private: /** - * Handles fatal errors in the application. + * Handles fatal errors in the application. */ void fatalErrorHandler(); /** - * Sets up the SerialMuxProt Server. + * Sets up the SerialMuxProt Server. * * @returns true if successful, otherwise false. */ bool setupSerialMuxProtServer(); /** - * Connects to the ROS2 server via TCP. + * Connects to the ROS2 server via TCP. * * @returns true if successful, otherwise false. */ bool connectToROS2Server(); /** - * Sends a newline-terminated TCP packet to the ROS2 server. + * Sends a newline-terminated TCP packet to the ROS2 server. * * @param[in] payload Payload to send. */ void sendPacket(const String& payload); /** - * Receives and buffers TCP packets from the ROS2 server. - * Processes complete newline-terminated lines. + * Receives and buffers TCP packets from the ROS2 server. + * Processes complete newline-terminated lines. */ void receivePackets(); @@ -192,14 +192,14 @@ class App void processIncomingLine(const String& line); /** - * Prohibits copy construction of an instance. + * Prohibits copy construction of an instance. * * @param[in] app Source instance. */ App(const App& app); /** - * Prohibits assignment of an instance. + * Prohibits assignment of an instance. * * @param[in] app Source instance. *