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/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/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..44685fe4 --- /dev/null +++ b/lib/APPSlam/src/App.cpp @@ -0,0 +1,520 @@ +/* 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 Jonas Hochhaus + */ + +/****************************************************************************** + * Includes + *****************************************************************************/ +#include "App.h" +#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 */ + +/** 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 if not defined by the build configuration */ +#ifndef APPSLAM_TCP_SERVER_PORT +#define APPSLAM_TCP_SERVER_PORT 8888U +#endif /* APPSLAM_TCP_SERVER_PORT */ + +/****************************************************************************** + * 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_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 + *****************************************************************************/ + +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 + { + if (false == connectToROS2Server()) + { + LOG_WARNING("Initial ROS2 server connection failed. Will retry in loop()."); + } + + 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_lastReconnectAttemptTimeMs) >= TCP_RECONNECT_INTERVAL_MS) + { + m_lastReconnectAttemptTimeMs = now; + + LOG_INFO("ROS2 server disconnected. Attempting reconnect..."); + m_tcpClient.stop(); + + if (false == connectToROS2Server()) + { + LOG_WARNING("ROS2 server 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::connectToROS2Server() +{ + bool isSuccessful = false; + IPAddress serverIp; + + if (false == serverIp.fromString(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("ROS2 server connected at %s:%u", APPSLAM_TCP_SERVER_IP, APPSLAM_TCP_SERVER_PORT); + isSuccessful = true; + } + else + { + LOG_WARNING("Failed to connect to ROS2 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'; + size_t bytesWritten = m_tcpClient.write(reinterpret_cast(packet.c_str()), packet.length()); + + 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."); + } +} + +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) + { + if (0 != m_tcpRxBuffer.length()) + { + processIncomingLine(m_tcpRxBuffer); + } + + m_tcpRxBuffer = String(); + } + else + { + m_tcpRxBuffer += c; + + if (m_tcpRxBuffer.length() >= MAX_LINE_LEN) + { + LOG_WARNING("TCP line too long, discarding."); + m_tcpRxBuffer = String(); + } + } + } + } + } 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[JSON_KEY_LINEAR].isNull()) && (!doc[JSON_KEY_ANGULAR].isNull())) + { + 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); + 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."); + } + } + 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; + if (0U < serializeJson(doc, out)) + { + sendPacket(out); + } + else + { + LOG_WARNING("Failed to serialize vehicle data."); + } + } +} + +/****************************************************************************** + * 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..0a554e2c --- /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 Jonas Hochhaus + * + * @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: + /** + * Constructs 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_lastReconnectAttemptTimeMs(0U), + m_statusTimer(), + m_isFatalError(false) + { + m_smpServer.setUserData(this); + } + + /** + * Destroys the SLAM application. + */ + ~App() + { + } + + /** + * Sets up the application. + */ + void setup(); + + /** + * Processes the application periodically. + */ + void loop(); + + /** + * Handles vehicle data received from SerialMuxProt and forwards it to the ROS2 server. + * + * @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 ROS2 server. */ + 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; + + /** Timestamp of the last TCP reconnect attempt [ms]. */ + uint32_t m_lastReconnectAttemptTimeMs; + + /** + * Timer for sending system status to RU. + */ + SimpleTimer m_statusTimer; + + /** + * Is fatal error happened? + */ + bool m_isFatalError; + +private: + /** + * Handles fatal errors in the application. + */ + void fatalErrorHandler(); + + /** + * Sets up the SerialMuxProt Server. + * + * @returns true if successful, otherwise false. + */ + bool setupSerialMuxProtServer(); + + /** + * 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. + * + * @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. + */ + void receivePackets(); + + /** + * 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. + * + * @param[in] line JSON string to parse and process. + */ + void processIncomingLine(const String& line); + + /** + * Prohibits copy construction of an instance. + * + * @param[in] app Source instance. + */ + App(const App& app); + + /** + * Prohibits assignment of an instance. + * + * @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..6d4692ff --- /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 Jonas Hochhaus + * + * @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/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/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..6891d36e 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 @@ -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 @@ -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 ; ***************************************************************************** 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 diff --git a/scripts/webots_launcher.py b/scripts/webots_launcher.py index 9ee12aa3..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 @@ -98,6 +98,70 @@ # 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) + + +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 ################################################################################ @@ -107,7 +171,8 @@ name="webots_launcher", dependencies=PROGRAM_PATH + PROGRAM_NAME, actions=[ - WEBOTS_LAUNCHER_ACTION + check_webots_ready, + launch_webots_controller, ], title="Launch", description="Launch application with Webots launcher."