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/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 ; *****************************************************************************