Merge ROS2 SLAM/NAV integration branch to develop

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

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 : <<use>>
 VCM ..> mqtt : <<use>>
 
-@enduml
+@enduml

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,14 +235,16 @@ 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);
     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_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<int32_t>();
+                    cmd.yPos        = yPos.as<int32_t>();
+                    cmd.orientation = heading.as<int32_t>();
+                }
                 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<uint8_t*>(&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

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

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": "*"
+}