From 5fcf6923a3945c8bb089f1864d7f840d97fa5043 Mon Sep 17 00:00:00 2001 From: jonpg01 Date: Thu, 21 May 2026 14:42:28 +0200 Subject: [PATCH 1/7] 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 2/7] 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 3/7] 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 4/7] 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 5/7] 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 6/7] 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 7/7] 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. *