botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 17:47:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

updates to topology with flatbuffers

Browse Source
This commit is contained in:
superkor
2026-01-23 18:51:06 -05:00
parent 54128ef410
commit 335a8dcd55
8 changed files with 245 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
components/flatbuffers/include/flatbuffers_generated/Movement_generated.h
View File

@@ -8,10 +8,10 @@
// Ensure the included flatbuffers.h is the same version as when this file was
// generated, otherwise it may not be compatible.
static_assert(FLATBUFFERS_VERSION_MAJOR == 25 &&
FLATBUFFERS_VERSION_MINOR == 12 &&
FLATBUFFERS_VERSION_REVISION == 19,
"Non-compatible flatbuffers version included");
// static_assert(FLATBUFFERS_VERSION_MAJOR == 25 &&
// FLATBUFFERS_VERSION_MINOR == 12 &&
// FLATBUFFERS_VERSION_REVISION == 19,
// "Non-compatible flatbuffers version included");
namespace Movement {

8
components/flatbuffers/include/flatbuffers_generated/Topology_generated.h
View File

@@ -8,10 +8,10 @@
// Ensure the included flatbuffers.h is the same version as when this file was
// generated, otherwise it may not be compatible.
static_assert(FLATBUFFERS_VERSION_MAJOR == 25 &&
FLATBUFFERS_VERSION_MINOR == 12 &&
FLATBUFFERS_VERSION_REVISION == 19,
"Non-compatible flatbuffers version included");
// static_assert(FLATBUFFERS_VERSION_MAJOR == 25 &&
// FLATBUFFERS_VERSION_MINOR == 12 &&
// FLATBUFFERS_VERSION_REVISION == 19,
// "Non-compatible flatbuffers version included");
namespace Topology {

210
components/movements/TopologyManager.cpp
View File

@@ -17,70 +17,182 @@ TopologyManager::~TopologyManager(){
nvs_close(handle);
}
// esp_err_t TopologyManager::add_board_to_topology(struct NeighbourBlob& blob){
// if (!ready){
// return ESP_ERR_INVALID_STATE;
// }
/**
* @brief Creates a `Topology::NeighbourBlob` based on the input parameters and adds the blob to the internal topology map
*
* @param connections This is a vector of pairs (uint8_t: `channel number`, uint16_t: `board_id`) representing the connection from the `curr_board_id` to `board_id` via `channel number`
* @param curr_board_id This is the board id, from its perspective, has the connections to the other boards in the vector `connections`
* @return esp_err_t
*
* @note
* Connections to fail:
*
* 1. Any of the `board_id` in the vector is `PC_ADDR` or `BROADCAST_ADDR` - Reserved addresses
*
* 2. Any of the `board_id` in the vector is `curr_board_id` - No loopback connections are allowed
*
* 3. Any of the `channel` in the vector is greater than or equal to `MAX_CHANNELS` - Channels are 0-indexed and there are at most 4 channels
*
* 4. All `board_id` is not unique in the vector - Should be a 1:1 connection between the boards
*/
esp_err_t TopologyManager::add_board_to_topology(std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id){
if (!ready){
return ESP_ERR_INVALID_STATE;
}
// if (blob.num_connections > MAX_CHANNELS || blob.curr_board_id == PC_ADDR || blob.curr_board_id == BROADCAST_ADDR){
// return ESP_ERR_INVALID_ARG;
// }
if (connections.size() == 0 || connections.size() > MAX_CHANNELS){
return ESP_ERR_INVALID_ARG;
}
// for (uint8_t i = 0; i < blob.num_connections; i++){
// ChannelBoardConn connection = blob.neighbour_connections[i];
// if (connection.channel > MAX_CHANNELS || connection.board_id == PC_ADDR
// || connection == BROADCAST_ADDR || connection.board_id == blob.curr_board_id){
// return ESP_ERR_INVALID_ARG;
// }
// }
if (curr_board_id == PC_ADDR || curr_board_id == BROADCAST_ADDR){
return ESP_ERR_INVALID_ARG;
}
// topology[blob.curr_board_id] = blob;
//verify the connections in the vector
std::unordered_set<uint8_t> check_set;
std::vector<Topology::ChannelBoardConn> conns;
for (const std::pair<uint8_t, uint16_t>& pair : connections){
if (pair.second >= MAX_CHANNELS || pair.first == curr_board_id || pair.first == PC_ADDR
|| pair.first == BROADCAST_ADDR || check_set.find(pair.first) != check_set.end()){
return ESP_ERR_INVALID_ARG;
}
check_set.insert(pair.first);
conns.push_back(builder.build_connections(pair.first, pair.second));
}
topology[curr_board_id] = conns;
// return ESP_OK;
// }
return ESP_OK;
}
// esp_err_t TopologyManager::remove_board_from_topology(uint16_t board_id){
// if (!ready){
// return ESP_ERR_INVALID_STATE;
// }
esp_err_t TopologyManager::remove_board_from_topology(uint16_t board_id){
if (!ready){
return ESP_ERR_INVALID_STATE;
}
// if (topology.find(board_id) == topology.end()){
// return ESP_ERR_NOT_FOUND;
// }
if (topology.find(board_id) == topology.end()){
return ESP_ERR_NOT_FOUND;
}
// topology.erase(board_id);
topology.erase(board_id);
// return ESP_OK;
// }
return ESP_OK;
}
// esp_err_t TopologyManager::get_board_in_topology(struct NeighbourBlob& blob){
// if (!ready){
// return ESP_ERR_INVALID_STATE;
// }
esp_err_t TopologyManager::get_board_in_topology(std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id){
if (!ready){
return ESP_ERR_INVALID_STATE;
}
// if (topology.find(board_id) == topology.end()){
// return ESP_ERR_NOT_FOUND;
// }
if (topology.find(curr_board_id) == topology.end()){
return ESP_ERR_NOT_FOUND;
}
// blob = topology.find(board_id);
std::vector<Topology::ChannelBoardConn> conn = topology[curr_board_id];
// return ESP_OK;
// }
connections.clear();
// esp_err_t TopologyManager::verify_topology(){
// if (!ready){
// return ESP_ERR_INVALID_STATE;
// }
// return ESP_OK;
// }
for (const Topology::ChannelBoardConn c : conn){
connections.push_back(std::pair<uint8_t, uint16_t>(c.channel(), c.board_id()));
}
// esp_err_t TopologyManager::get_curr_topology(struct Topology& topology){
// if (!ready){
// return ESP_ERR_INVALID_STATE;
// }
return ESP_OK;
}
// return ESP_OK;
// }
esp_err_t TopologyManager::verify_topology(){
if (!ready){
return ESP_ERR_INVALID_STATE;
}
if (topology.size() == 0){
return ESP_ERR_INVALID_STATE;
}
/**
* Conditions to fail:
* 1. Any board that is referenced in the topology does not have a reciprocal connection (eg. A -> B and B -> A)
* 2. The topology contains 2 separate graphs (a board in the topology should be in the same graph with some sort of path to all other boards)
*/
//condition 1
for (const auto& pair : topology){
for (const Topology::ChannelBoardConn& conn : pair.second){
if (topology.find(conn.board_id()) == topology.end()){
return ESP_ERR_INVALID_STATE; //could not find reciprocal board
}
std::vector<Topology::ChannelBoardConn> topology_board_conn = topology[conn.board_id()];
bool found = false;
for (const Topology::ChannelBoardConn& c : topology_board_conn){
if (c.board_id() == pair.first){
found = true;
break;
}
}
if (!found){
return ESP_ERR_INVALID_STATE; //reciprocal board does not have a connection back to `pair.first`
}
}
}
//condition 2 - dfs
std::unordered_set<uint16_t> visited;
std::stack<uint16_t> backtrack;
uint32_t count = 0;
backtrack.push(topology.begin()->first);
while (!backtrack.empty()){
uint16_t curr_node = backtrack.top();
backtrack.pop();
if (visited.find(curr_node) != visited.end()) {
continue;
}
visited.insert(curr_node);
count++;
const std::vector<Topology::ChannelBoardConn>& conns = topology[curr_node];
for (const Topology::ChannelBoardConn& conn : conns) {
uint16_t next = conn.board_id();
if (visited.find(next) != visited.end()) {
backtrack.push(next);
}
}
}
if (count != topology.size()){
return ESP_ERR_INVALID_STATE;
}
return ESP_OK;
}
esp_err_t TopologyManager::get_curr_topology(std::unordered_map<uint16_t, std::vector<std::pair<uint8_t, uint16_t>>>& topology){
if (!ready){
return ESP_ERR_INVALID_STATE;
}
topology.clear();
for (const auto& [key, value] : this->topology){
topology[key] = std::vector<std::pair<uint8_t, uint16_t>>();
for (const Topology::ChannelBoardConn c : value){
topology[key].push_back(std::pair<uint8_t, uint16_t>(c.channel(), c.board_id()));
}
}
return ESP_OK;
}
// esp_err_t TopologyManager::write_nvs_topology(){
// if (!ready){

27
components/movements/include/TopologyManager.h
View File

@@ -1,31 +1,36 @@
#include "MovementManager.h"
#ifdef MOVEMENTS
#include "nvs_flash.h"
#include <unordered_map>
#include <unordered_set>
// move these to constants header file?
#include "RMTManager.h"
#include "DataLinkManager.h"
#include "Frames.h"
#ifdef MOVEMENTS
#pragma once
#include "TopologyBuilder.h"
#include <vector>
#include <stack>
#include <algorithm>
class TopologyManager {
public:
TopologyManager();
~TopologyManager();
// esp_err_t add_board_to_topology(struct NeighbourBlob& blob);
// esp_err_t remove_board_from_topology(uint16_t board_id);
// esp_err_t get_board_in_topology(struct NeighbourBlob& blob);
// esp_err_t verify_topology();
// esp_err_t get_curr_topology(struct Topology& topology);
// esp_err_t write_nvs_topology();
// esp_err_t get_nvs_topology(struct Topology& topology);
esp_err_t add_board_to_topology(std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id);
esp_err_t remove_board_from_topology(uint16_t board_id);
esp_err_t get_board_in_topology(std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id);
esp_err_t verify_topology();
esp_err_t get_curr_topology(std::unordered_map<uint16_t, std::vector<std::pair<uint8_t, uint16_t>>>& topology);
esp_err_t write_nvs_topology();
esp_err_t get_nvs_topology(std::unordered_map<uint16_t, std::vector<Topology::ChannelBoardConn>>& topology);
private:
// std::unordered_map<uint16_t, struct NeighbourBlob> topology;
std::unordered_map<uint16_t, std::vector<Topology::ChannelBoardConn>> topology;
nvs_handle_t handle;
bool ready = false;
Flatbuffers::TopologyBuilder builder;
};
#endif //MOVEMENTS