mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-12 09:27:35 +08:00
msg: add message translation node for ROS
This commit is contained in:
Beat Küng
@@ -0,0 +1,623 @@
|
||||
/****************************************************************************
|
||||
* Copyright (c) 2024 PX4 Development Team.
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <src/graph.h>
|
||||
|
||||
|
||||
TEST(graph, basic)
|
||||
{
|
||||
struct NodeData {
|
||||
bool iterated{false};
|
||||
bool translated{false};
|
||||
};
|
||||
Graph<NodeData> graph;
|
||||
|
||||
const int32_t message1_value = 3;
|
||||
const int32_t offset = 4;
|
||||
|
||||
// Add 2 nodes
|
||||
const MessageIdentifier id1{"topic_name", 1};
|
||||
auto buffer1 = std::make_shared<int32_t>();
|
||||
*buffer1 = message1_value;
|
||||
EXPECT_TRUE(graph.addNodeIfNotExists(id1, {}, buffer1));
|
||||
EXPECT_FALSE(graph.addNodeIfNotExists(id1, {}, std::make_shared<int32_t>()));
|
||||
const MessageIdentifier id2{"topic_name", 4};
|
||||
auto buffer2 = std::make_shared<int64_t>();
|
||||
*buffer2 = 773;
|
||||
EXPECT_TRUE(graph.addNodeIfNotExists(id2, {}, buffer2));
|
||||
|
||||
// Search nodes
|
||||
EXPECT_TRUE(graph.findNode(id1).has_value());
|
||||
EXPECT_TRUE(graph.findNode(id2).has_value());
|
||||
|
||||
// Add 1 translation
|
||||
auto translation_cb = [&offset](const std::vector<MessageBuffer>& a, std::vector<MessageBuffer>& b) {
|
||||
auto a_value = static_cast<const int32_t*>(a[0].get());
|
||||
auto b_value = static_cast<int64_t*>(b[0].get());
|
||||
*b_value = *a_value + offset;
|
||||
};
|
||||
graph.addTranslation(translation_cb, {id1}, {id2});
|
||||
|
||||
// Iteration from id1 must reach id2
|
||||
auto node1 = graph.findNode(id1).value();
|
||||
auto node2 = graph.findNode(id2).value();
|
||||
auto iterate_cb = [](const Graph<NodeData>::MessageNodePtr& node) {
|
||||
node->data().iterated = true;
|
||||
};
|
||||
graph.iterateBFS(node1, iterate_cb);
|
||||
EXPECT_TRUE(node1->data().iterated);
|
||||
EXPECT_TRUE(node2->data().iterated);
|
||||
node1->data().iterated = false;
|
||||
node2->data().iterated = false;
|
||||
|
||||
// Iteration from id2 must not reach id1
|
||||
graph.iterateBFS(node2, iterate_cb);
|
||||
EXPECT_FALSE(node1->data().iterated);
|
||||
EXPECT_TRUE(node2->data().iterated);
|
||||
|
||||
// Test translation
|
||||
graph.translate(node1,
|
||||
[](auto&& node) {
|
||||
assert(!node->data().translated);
|
||||
node->data().translated = true;
|
||||
});
|
||||
EXPECT_FALSE(node1->data().translated);
|
||||
EXPECT_TRUE(node2->data().translated);
|
||||
EXPECT_EQ(*buffer1, message1_value);
|
||||
EXPECT_EQ(*buffer2, message1_value + offset);
|
||||
}
|
||||
|
||||
|
||||
TEST(graph, multi_path)
|
||||
{
|
||||
// Multiple paths with cycles
|
||||
struct NodeData {
|
||||
unsigned iterated_idx{0};
|
||||
bool translated{false};
|
||||
};
|
||||
Graph<NodeData> graph;
|
||||
|
||||
static constexpr unsigned num_nodes = 6;
|
||||
std::array<MessageIdentifier, num_nodes> ids{{
|
||||
{"topic_name", 1},
|
||||
{"topic_name", 2},
|
||||
{"topic_name", 3},
|
||||
{"topic_name", 4},
|
||||
{"topic_name", 5},
|
||||
{"topic_name", 6},
|
||||
}};
|
||||
|
||||
std::array<std::shared_ptr<int32_t>, num_nodes> buffers{{
|
||||
std::make_shared<int32_t>(),
|
||||
std::make_shared<int32_t>(),
|
||||
std::make_shared<int32_t>(),
|
||||
std::make_shared<int32_t>(),
|
||||
std::make_shared<int32_t>(),
|
||||
std::make_shared<int32_t>(),
|
||||
}};
|
||||
|
||||
// Nodes
|
||||
for (unsigned i = 0; i < num_nodes; ++i) {
|
||||
EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i]));
|
||||
}
|
||||
|
||||
// Translations
|
||||
std::bitset<32> translated;
|
||||
|
||||
auto get_translation_cb = [&translated](unsigned bit) {
|
||||
auto translation_cb = [&translated, bit](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
auto a_value = static_cast<const int32_t *>(a[0].get());
|
||||
auto b_value = static_cast<int32_t *>(b[0].get());
|
||||
*b_value = *a_value | (1 << bit);
|
||||
translated.set(bit);
|
||||
};
|
||||
return translation_cb;
|
||||
};
|
||||
|
||||
// Graph:
|
||||
// ___ 2 -- 3 -- 4
|
||||
// | |
|
||||
// 1 _______|
|
||||
// |
|
||||
// 5
|
||||
// |
|
||||
// 6
|
||||
|
||||
unsigned next_bit = 0;
|
||||
// Connect each node to the previous and next, except the last 3
|
||||
for (unsigned i=0; i < num_nodes - 3; ++i) {
|
||||
graph.addTranslation(get_translation_cb(next_bit++), {ids[i]}, {ids[i+1]});
|
||||
graph.addTranslation(get_translation_cb(next_bit++), {ids[i+1]}, {ids[i]});
|
||||
}
|
||||
|
||||
// Connect the first to the 3rd as well
|
||||
graph.addTranslation(get_translation_cb(next_bit++), {ids[0]}, {ids[2]});
|
||||
graph.addTranslation(get_translation_cb(next_bit++), {ids[2]}, {ids[0]});
|
||||
|
||||
// Connect the second last to the first one
|
||||
graph.addTranslation(get_translation_cb(next_bit++), {ids[0]}, {ids[num_nodes-2]});
|
||||
graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-2]}, {ids[0]});
|
||||
|
||||
// Connect the second last to the last one
|
||||
graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-1]}, {ids[num_nodes-2]});
|
||||
graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-2]}, {ids[num_nodes-1]});
|
||||
|
||||
unsigned iteration_idx = 1;
|
||||
graph.iterateBFS(graph.findNode(ids[0]).value(), [&iteration_idx](const Graph<NodeData>::MessageNodePtr& node) {
|
||||
assert(node->data().iterated_idx == 0);
|
||||
node->data().iterated_idx = iteration_idx++;
|
||||
});
|
||||
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().iterated_idx, 1);
|
||||
// We're a bit stricter than we would have to be: ids[1,2,4] would be allowed to have any of the values (2,3,4)
|
||||
EXPECT_EQ(graph.findNode(ids[1]).value()->data().iterated_idx, 2);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().iterated_idx, 3);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().iterated_idx, 4);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().iterated_idx, 5);
|
||||
|
||||
|
||||
// Translation
|
||||
graph.translate(graph.findNode(ids[0]).value(),
|
||||
[](auto&& node) {
|
||||
assert(!node->data().translated);
|
||||
node->data().translated = true;
|
||||
});
|
||||
|
||||
// All nodes should be translated except the first
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false);
|
||||
for (unsigned i = 1; i < num_nodes; ++i) {
|
||||
EXPECT_EQ(graph.findNode(ids[i]).value()->data().translated, true) << "node[" << i << "]";
|
||||
}
|
||||
|
||||
// Ensure the correct edges were used for translations
|
||||
EXPECT_EQ("00000000000000000000100101010001", translated.to_string());
|
||||
|
||||
// Ensure correct translation path taken for each node (which is stored in the buffers),
|
||||
// and translation callback got called
|
||||
EXPECT_EQ(*buffers[0], 0);
|
||||
EXPECT_EQ(*buffers[1], 0b1);
|
||||
EXPECT_EQ(*buffers[2], 0b1000000);
|
||||
EXPECT_EQ(*buffers[3], 0b1010000);
|
||||
EXPECT_EQ(*buffers[4], 0b100000000);
|
||||
EXPECT_EQ(*buffers[5], 0b100100000000);
|
||||
|
||||
for (unsigned i=0; i < num_nodes; ++i) {
|
||||
printf("node[%i]: translated: %i, buffer: %i\n", i, graph.findNode(ids[i]).value()->data().translated,
|
||||
*buffers[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(graph, multi_links) {
|
||||
// Multiple topics (merging / splitting)
|
||||
struct NodeData {
|
||||
bool translated{false};
|
||||
};
|
||||
Graph<NodeData> graph;
|
||||
|
||||
static constexpr unsigned num_nodes = 6;
|
||||
std::array<MessageIdentifier, num_nodes> ids{{
|
||||
{"topic1", 1},
|
||||
{"topic2", 1},
|
||||
{"topic1", 2},
|
||||
{"topic3", 1},
|
||||
{"topic4", 1},
|
||||
{"topic1", 3},
|
||||
}};
|
||||
|
||||
std::array<std::shared_ptr<uint32_t>, num_nodes> buffers{{
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
}};
|
||||
|
||||
// Nodes
|
||||
for (unsigned i = 0; i < num_nodes; ++i) {
|
||||
EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i]));
|
||||
}
|
||||
|
||||
|
||||
// Graph
|
||||
// ___
|
||||
// 1 - | | ---
|
||||
// | | - 3 - | | - 6
|
||||
// 2 - | | ---
|
||||
// | ---
|
||||
// | ___
|
||||
// --- | | - 4
|
||||
// | | - 5
|
||||
// ---
|
||||
|
||||
// Translations
|
||||
auto translation_cb_merge = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 2);
|
||||
assert(b.size() == 1);
|
||||
auto a_value1 = static_cast<const uint32_t *>(a[0].get());
|
||||
auto a_value2 = static_cast<const uint32_t *>(a[1].get());
|
||||
auto b_value = static_cast<uint32_t *>(b[0].get());
|
||||
*b_value = *a_value1 | *a_value2;
|
||||
};
|
||||
auto translation_cb_split = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 1);
|
||||
assert(b.size() == 2);
|
||||
auto a_value = static_cast<const uint32_t *>(a[0].get());
|
||||
auto b_value1 = static_cast<uint32_t *>(b[0].get());
|
||||
auto b_value2 = static_cast<uint32_t *>(b[1].get());
|
||||
*b_value1 = *a_value & 0x0000ffffu;
|
||||
*b_value2 = *a_value & 0xffff0000u;
|
||||
};
|
||||
auto translation_cb_direct = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 1);
|
||||
assert(b.size() == 1);
|
||||
auto a_value = static_cast<const uint32_t *>(a[0].get());
|
||||
auto b_value = static_cast<uint32_t *>(b[0].get());
|
||||
*b_value = *a_value;
|
||||
};
|
||||
|
||||
auto addTranslation = [&](const std::vector<MessageIdentifier>& inputs, const std::vector<MessageIdentifier>& outputs) {
|
||||
assert(inputs.size() <= 2);
|
||||
assert(outputs.size() <= 2);
|
||||
if (inputs.size() == 1) {
|
||||
if (outputs.size() == 1) {
|
||||
graph.addTranslation(translation_cb_direct, inputs, outputs);
|
||||
graph.addTranslation(translation_cb_direct, outputs, inputs);
|
||||
} else {
|
||||
graph.addTranslation(translation_cb_split, inputs, outputs);
|
||||
graph.addTranslation(translation_cb_merge, outputs, inputs);
|
||||
}
|
||||
} else {
|
||||
assert(outputs.size() == 1);
|
||||
graph.addTranslation(translation_cb_merge, inputs, outputs);
|
||||
graph.addTranslation(translation_cb_split, outputs, inputs);
|
||||
}
|
||||
};
|
||||
addTranslation({ids[0], ids[1]}, {ids[2]});
|
||||
addTranslation({ids[1]}, {ids[3], ids[4]});
|
||||
addTranslation({ids[2]}, {ids[5]});
|
||||
|
||||
auto translate_node = [&](const MessageIdentifier& id) {
|
||||
graph.translate(graph.findNode(id).value(),
|
||||
[](auto&& node) {
|
||||
assert(!node->data().translated);
|
||||
node->data().translated = true;
|
||||
});
|
||||
|
||||
};
|
||||
auto reset_translated = [&]() {
|
||||
for (const auto& id : ids) {
|
||||
graph.findNode(id).value()->data().translated = false;
|
||||
}
|
||||
};
|
||||
|
||||
// Updating node 2 should trigger an output for nodes 4+5 (splitting)
|
||||
*buffers[0] = 0xa00000b0;
|
||||
*buffers[1] = 0x0f00000f;
|
||||
translate_node(ids[1]);
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false);
|
||||
EXPECT_EQ(*buffers[3], 0x0000000f);
|
||||
EXPECT_EQ(*buffers[4], 0x0f000000);
|
||||
|
||||
reset_translated();
|
||||
|
||||
// Now updating node 1 should update nodes 3+6 (merging, both inputs available now)
|
||||
translate_node(ids[0]);
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true);
|
||||
EXPECT_EQ(*buffers[2], 0xaf0000bf);
|
||||
EXPECT_EQ(*buffers[5], 0xaf0000bf);
|
||||
|
||||
reset_translated();
|
||||
|
||||
// Another update must not trigger any other updates
|
||||
translate_node(ids[0]);
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false);
|
||||
|
||||
reset_translated();
|
||||
|
||||
// Backwards: updating node 6 should trigger updates for 1+2, but also 4+5
|
||||
*buffers[5] = 0xc00000d0;
|
||||
translate_node(ids[5]);
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false);
|
||||
EXPECT_EQ(*buffers[0], 0x000000d0);
|
||||
EXPECT_EQ(*buffers[1], 0xc0000000);
|
||||
EXPECT_EQ(*buffers[2], 0xc00000d0);
|
||||
EXPECT_EQ(*buffers[3], 0);
|
||||
EXPECT_EQ(*buffers[4], 0xc0000000);
|
||||
EXPECT_EQ(*buffers[5], 0xc00000d0);
|
||||
}
|
||||
|
||||
TEST(graph, multi_links2) {
|
||||
// Multiple topics (merging / splitting)
|
||||
struct NodeData {
|
||||
bool translated{false};
|
||||
};
|
||||
Graph<NodeData> graph;
|
||||
|
||||
static constexpr unsigned num_nodes = 8;
|
||||
std::array<MessageIdentifier, num_nodes> ids{{
|
||||
{"topic1", 1},
|
||||
{"topic2", 1},
|
||||
{"topic3", 1},
|
||||
{"topic1", 2},
|
||||
{"topic2", 2},
|
||||
{"topic1", 3},
|
||||
{"topic2", 3},
|
||||
{"topic3", 3},
|
||||
}};
|
||||
|
||||
std::array<std::shared_ptr<uint32_t>, num_nodes> buffers{{
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
}};
|
||||
|
||||
// Nodes
|
||||
for (unsigned i = 0; i < num_nodes; ++i) {
|
||||
EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i]));
|
||||
}
|
||||
|
||||
|
||||
// Graph
|
||||
// ___ ___
|
||||
// 1 - | | | | - 6
|
||||
// | | - 4 - | |
|
||||
// 2 - | | | | - 7
|
||||
// | | - 5 - | |
|
||||
// 3 - | | | | - 8
|
||||
// --- ---
|
||||
|
||||
// Translations
|
||||
auto translation_cb_32 = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 3);
|
||||
assert(b.size() == 2);
|
||||
auto a_value1 = static_cast<const uint32_t *>(a[0].get());
|
||||
auto a_value2 = static_cast<const uint32_t *>(a[1].get());
|
||||
auto a_value3 = static_cast<const uint32_t *>(a[2].get());
|
||||
auto b_value1 = static_cast<uint32_t *>(b[0].get());
|
||||
auto b_value2 = static_cast<uint32_t *>(b[1].get());
|
||||
*b_value1 = *a_value1 | *a_value2;
|
||||
*b_value2 = *a_value3;
|
||||
};
|
||||
auto translation_cb_23 = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 2);
|
||||
assert(b.size() == 3);
|
||||
auto a_value1 = static_cast<const uint32_t *>(a[0].get());
|
||||
auto a_value2 = static_cast<const uint32_t *>(a[1].get());
|
||||
auto b_value1 = static_cast<uint32_t *>(b[0].get());
|
||||
auto b_value2 = static_cast<uint32_t *>(b[1].get());
|
||||
auto b_value3 = static_cast<uint32_t *>(b[2].get());
|
||||
*b_value1 = *a_value1 & 0x0000ffffu;
|
||||
*b_value2 = *a_value1 & 0xffff0000u;
|
||||
*b_value3 = *a_value2;
|
||||
};
|
||||
graph.addTranslation(translation_cb_32, {ids[0], ids[1], ids[2]}, {ids[3], ids[4]});
|
||||
graph.addTranslation(translation_cb_23, {ids[3], ids[4]}, {ids[0], ids[1], ids[2]});
|
||||
|
||||
graph.addTranslation(translation_cb_23, {ids[3], ids[4]}, {ids[5], ids[6], ids[7]});
|
||||
graph.addTranslation(translation_cb_32, {ids[5], ids[6], ids[7]}, {ids[3], ids[4]});
|
||||
|
||||
|
||||
auto translate_node = [&](const MessageIdentifier& id) {
|
||||
graph.translate(graph.findNode(id).value(),
|
||||
[](auto&& node) {
|
||||
assert(!node->data().translated);
|
||||
node->data().translated = true;
|
||||
});
|
||||
};
|
||||
auto reset_translated = [&]() {
|
||||
for (const auto& id : ids) {
|
||||
graph.findNode(id).value()->data().translated = false;
|
||||
}
|
||||
};
|
||||
|
||||
// Updating nodes 1+2+3 should update nodes 6+7+8
|
||||
*buffers[0] = 0xa00000b0;
|
||||
*buffers[1] = 0x0f00000f;
|
||||
*buffers[2] = 0x0c00000c;
|
||||
translate_node(ids[1]);
|
||||
translate_node(ids[0]);
|
||||
translate_node(ids[2]);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[7]).value()->data().translated, true);
|
||||
EXPECT_EQ(*buffers[3], 0xa00000b0 | 0x0f00000f);
|
||||
EXPECT_EQ(*buffers[4], 0x0c00000c);
|
||||
EXPECT_EQ(*buffers[5], (0xa00000b0 | 0x0f00000f) & 0x0000ffffu);
|
||||
EXPECT_EQ(*buffers[6], (0xa00000b0 | 0x0f00000f) & 0xffff0000u);
|
||||
EXPECT_EQ(*buffers[7], 0x0c00000c);
|
||||
|
||||
reset_translated();
|
||||
|
||||
// Now updating nodes 6+7+8 should update nodes 1+2+3
|
||||
*buffers[5] = 0xa00000b0;
|
||||
*buffers[6] = 0x0f00000f;
|
||||
*buffers[7] = 0x0c00000c;
|
||||
translate_node(ids[5]);
|
||||
translate_node(ids[6]);
|
||||
translate_node(ids[7]);
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true);
|
||||
EXPECT_EQ(*buffers[3], 0xa00000b0 | 0x0f00000f);
|
||||
EXPECT_EQ(*buffers[4], 0x0c00000c);
|
||||
EXPECT_EQ(*buffers[0], (0xa00000b0 | 0x0f00000f) & 0x0000ffffu);
|
||||
EXPECT_EQ(*buffers[1], (0xa00000b0 | 0x0f00000f) & 0xffff0000u);
|
||||
EXPECT_EQ(*buffers[2], 0x0c00000c);
|
||||
}
|
||||
|
||||
TEST(graph, multi_links3) {
|
||||
// Multiple topics (cannot use the shortest path)
|
||||
struct NodeData {
|
||||
bool translated{false};
|
||||
};
|
||||
Graph<NodeData> graph;
|
||||
|
||||
static constexpr unsigned num_nodes = 7;
|
||||
std::array<MessageIdentifier, num_nodes> ids{{
|
||||
{"topic1", 1},
|
||||
{"topic2", 1},
|
||||
{"topic1", 2},
|
||||
{"topic1", 3},
|
||||
{"topic1", 4},
|
||||
{"topic2", 4},
|
||||
{"topic1", 5},
|
||||
}};
|
||||
|
||||
std::array<std::shared_ptr<uint32_t>, num_nodes> buffers{{
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
std::make_shared<uint32_t>(),
|
||||
}};
|
||||
|
||||
// Nodes
|
||||
for (unsigned i = 0; i < num_nodes; ++i) {
|
||||
EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i]));
|
||||
}
|
||||
|
||||
|
||||
// Graph
|
||||
// ___ ___ ___ ___
|
||||
// 1 - | | - 3 - | | - 4 - | | - 5 - | | - 7
|
||||
// | | --- --- | |
|
||||
// | | | |
|
||||
// 2 - | | --------------------- 6 - | |
|
||||
// --- ---
|
||||
|
||||
// Translations
|
||||
auto translation_cb_21 = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 2);
|
||||
assert(b.size() == 1);
|
||||
auto a_value1 = static_cast<const uint32_t *>(a[0].get());
|
||||
auto a_value2 = static_cast<const uint32_t *>(a[1].get());
|
||||
auto b_value1 = static_cast<uint32_t *>(b[0].get());
|
||||
*b_value1 = *a_value1 | *a_value2;
|
||||
};
|
||||
auto translation_cb_22 = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 2);
|
||||
assert(b.size() == 2);
|
||||
auto a_value1 = static_cast<const uint32_t *>(a[0].get());
|
||||
auto a_value2 = static_cast<const uint32_t *>(a[1].get());
|
||||
auto b_value1 = static_cast<uint32_t *>(b[0].get());
|
||||
auto b_value2 = static_cast<uint32_t *>(b[1].get());
|
||||
*b_value1 = *a_value1;
|
||||
*b_value2 = *a_value2;
|
||||
};
|
||||
auto translation_cb_12 = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 1);
|
||||
assert(b.size() == 2);
|
||||
auto a_value1 = static_cast<const uint32_t *>(a[0].get());
|
||||
auto b_value1 = static_cast<uint32_t *>(b[0].get());
|
||||
auto b_value2 = static_cast<uint32_t *>(b[1].get());
|
||||
*b_value1 = *a_value1 & 0x0000ffffu;
|
||||
*b_value2 = *a_value1 & 0xffff0000u;
|
||||
};
|
||||
auto translation_cb_11 = [](const std::vector<MessageBuffer> &a, std::vector<MessageBuffer> &b) {
|
||||
assert(a.size() == 1);
|
||||
assert(b.size() == 1);
|
||||
auto a_value1 = static_cast<const uint32_t *>(a[0].get());
|
||||
auto b_value1 = static_cast<uint32_t *>(b[0].get());
|
||||
*b_value1 = *a_value1 + 1;
|
||||
};
|
||||
graph.addTranslation(translation_cb_22, {ids[0], ids[1]}, {ids[2], ids[5]});
|
||||
graph.addTranslation(translation_cb_22, {ids[2], ids[5]}, {ids[0], ids[1]});
|
||||
graph.addTranslation(translation_cb_11, {ids[2]}, {ids[3]});
|
||||
graph.addTranslation(translation_cb_11, {ids[3]}, {ids[2]});
|
||||
graph.addTranslation(translation_cb_11, {ids[3]}, {ids[4]});
|
||||
graph.addTranslation(translation_cb_11, {ids[4]}, {ids[3]});
|
||||
graph.addTranslation(translation_cb_21, {ids[4], ids[5]}, {ids[6]});
|
||||
graph.addTranslation(translation_cb_12, {ids[6]}, {ids[4], ids[5]});
|
||||
|
||||
|
||||
auto translate_node = [&](const MessageIdentifier& id) {
|
||||
graph.translate(graph.findNode(id).value(),
|
||||
[](auto&& node) {
|
||||
assert(!node->data().translated);
|
||||
assert(!node->data().translated);
|
||||
node->data().translated = true;
|
||||
});
|
||||
};
|
||||
auto reset_translated = [&]() {
|
||||
for (const auto& id : ids) {
|
||||
graph.findNode(id).value()->data().translated = false;
|
||||
}
|
||||
};
|
||||
|
||||
// Updating nodes 1+2 should update node 7
|
||||
*buffers[0] = 0xa00000b0;
|
||||
*buffers[1] = 0x0a00000b;
|
||||
translate_node(ids[1]);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, false);
|
||||
translate_node(ids[0]);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true);
|
||||
EXPECT_EQ(*buffers[2], 0xa00000b0);
|
||||
EXPECT_EQ(*buffers[3], 0xa00000b0 + 1);
|
||||
EXPECT_EQ(*buffers[4], 0xa00000b0 + 2);
|
||||
EXPECT_EQ(*buffers[5], 0x0a00000b);
|
||||
EXPECT_EQ(*buffers[6], ((0xa00000b0 + 2) | 0x0a00000b));
|
||||
|
||||
reset_translated();
|
||||
|
||||
// Now updating nodes 4+6 should update the rest
|
||||
*buffers[3] = 0xa00000b0;
|
||||
*buffers[5] = 0x0f00000f;
|
||||
translate_node(ids[3]);
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false);
|
||||
EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, false);
|
||||
translate_node(ids[5]);
|
||||
EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true);
|
||||
EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true);
|
||||
EXPECT_EQ(*buffers[0], 0xa00000b0 + 1);
|
||||
EXPECT_EQ(*buffers[1], 0x0f00000f);
|
||||
EXPECT_EQ(*buffers[2], 0xa00000b0 + 1);
|
||||
EXPECT_EQ(*buffers[4], 0xa00000b0 + 1);
|
||||
EXPECT_EQ(*buffers[6], (0xa00000b0 + 1) | 0x0f00000f);
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
/****************************************************************************
|
||||
* Copyright (c) 2024 PX4 Development Team.
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
const int ret = RUN_ALL_TESTS();
|
||||
rclcpp::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,350 @@
|
||||
/****************************************************************************
|
||||
* Copyright (c) 2024 PX4 Development Team.
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <src/monitor.h>
|
||||
#include <src/pub_sub_graph.h>
|
||||
#include <src/translation_util.h>
|
||||
|
||||
#include <std_msgs/msg/float32.hpp>
|
||||
#include <std_msgs/msg/float64.hpp>
|
||||
#include <std_msgs/msg/int64.hpp>
|
||||
#include <std_msgs/msg/color_rgba.hpp>
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// Define a custom struct with MESSAGE_VERSION field that can be used in ROS pubs and subs
|
||||
#define DEFINE_VERSIONED_ROS_MESSAGE_TYPE(CUSTOM_TYPE_NAME, ROS_TYPE_NAME, THIS_MESSAGE_VERSION) \
|
||||
struct CUSTOM_TYPE_NAME : public ROS_TYPE_NAME { \
|
||||
CUSTOM_TYPE_NAME() = default; \
|
||||
CUSTOM_TYPE_NAME(const ROS_TYPE_NAME& msg) : ROS_TYPE_NAME(msg) {} \
|
||||
static constexpr uint32_t MESSAGE_VERSION = THIS_MESSAGE_VERSION; \
|
||||
}; \
|
||||
template<> \
|
||||
struct rclcpp::TypeAdapter<CUSTOM_TYPE_NAME, ROS_TYPE_NAME> \
|
||||
{ \
|
||||
using is_specialized = std::true_type; \
|
||||
using custom_type = CUSTOM_TYPE_NAME; \
|
||||
using ros_message_type = ROS_TYPE_NAME; \
|
||||
static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) \
|
||||
{ \
|
||||
destination = source; \
|
||||
} \
|
||||
static void convert_to_custom(const ros_message_type & source, custom_type & destination) \
|
||||
{ \
|
||||
destination = source; \
|
||||
} \
|
||||
}; \
|
||||
RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(CUSTOM_TYPE_NAME, ROS_TYPE_NAME);
|
||||
|
||||
class PubSubGraphTest : public testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
_test_node = std::make_shared<rclcpp::Node>("test_node");
|
||||
_app_node = std::make_shared<rclcpp::Node>("app_node");
|
||||
_executor.add_node(_test_node);
|
||||
_executor.add_node(_app_node);
|
||||
|
||||
for (auto& node : {_app_node, _test_node}) {
|
||||
auto ret = rcutils_logging_set_logger_level(
|
||||
node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
RCLCPP_ERROR(
|
||||
node->get_logger(), "Error setting severity: %s",
|
||||
rcutils_get_error_string().str);
|
||||
rcutils_reset_error();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool spinWithTimeout(const std::function<bool(void)>& predicate) {
|
||||
const auto start = _app_node->now();
|
||||
while (_app_node->now() - start < 5s) {
|
||||
_executor.spin_some();
|
||||
if (predicate()) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp::Node> _test_node;
|
||||
std::shared_ptr<rclcpp::Node> _app_node;
|
||||
rclcpp::executors::SingleThreadedExecutor _executor;
|
||||
};
|
||||
|
||||
class RegisteredTranslationsTest : public RegisteredTranslations {
|
||||
public:
|
||||
RegisteredTranslationsTest() = default;
|
||||
};
|
||||
|
||||
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(Float32Versioned, std_msgs::msg::Float32, 1u);
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(ColorRGBAVersioned, std_msgs::msg::ColorRGBA, 2u);
|
||||
|
||||
class DirectTranslationTest {
|
||||
public:
|
||||
using MessageOlder = Float32Versioned;
|
||||
using MessageNewer = ColorRGBAVersioned;
|
||||
|
||||
static constexpr const char* kTopic = "test/direct_translation";
|
||||
|
||||
static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
|
||||
msg_newer.r = 1.f;
|
||||
msg_newer.g = msg_older.data;
|
||||
msg_newer.b = 2.f;
|
||||
}
|
||||
|
||||
static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
|
||||
msg_older.data = msg_newer.r + msg_newer.g + msg_newer.b;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
TEST_F(PubSubGraphTest, DirectTranslation)
|
||||
{
|
||||
RegisteredTranslationsTest registered_translations;
|
||||
registered_translations.registerDirectTranslation<DirectTranslationTest>();
|
||||
|
||||
PubSubGraph graph(*_test_node, registered_translations.topicTranslations());
|
||||
Monitor monitor(*_test_node, &graph, nullptr);
|
||||
|
||||
const std::string topic_name = DirectTranslationTest::kTopic;
|
||||
const std::string topic_name_older_version = getVersionedTopicName(topic_name, DirectTranslationTest::MessageOlder::MESSAGE_VERSION);
|
||||
const std::string topic_name_newer_version = getVersionedTopicName(topic_name, DirectTranslationTest::MessageNewer::MESSAGE_VERSION);
|
||||
|
||||
{
|
||||
// Create publisher + subscriber
|
||||
int num_topic_updates = 0;
|
||||
DirectTranslationTest::MessageNewer latest_data{};
|
||||
auto publisher = _app_node->create_publisher<DirectTranslationTest::MessageOlder>(topic_name_older_version,
|
||||
rclcpp::QoS(1).best_effort());
|
||||
auto subscriber = _app_node->create_subscription<DirectTranslationTest::MessageNewer>(topic_name_newer_version,
|
||||
rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data, this](
|
||||
DirectTranslationTest::MessageNewer::UniquePtr msg) -> void {
|
||||
RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated: %.3f", (double) msg->g);
|
||||
latest_data = *msg;
|
||||
++num_topic_updates;
|
||||
});
|
||||
|
||||
monitor.updateNow();
|
||||
|
||||
// Wait until there is a subscriber & publisher
|
||||
ASSERT_TRUE(spinWithTimeout([&subscriber, &publisher]() {
|
||||
return subscriber->get_publisher_count() > 0 && publisher->get_subscription_count() > 0;
|
||||
})) << "Timeout, no publisher/subscriber found";
|
||||
|
||||
// Publish some data & wait for it to arrive
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
DirectTranslationTest::MessageOlder msg_older;
|
||||
msg_older.data = (float) i;
|
||||
publisher->publish(msg_older);
|
||||
|
||||
ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() {
|
||||
return num_topic_updates == i + 1;
|
||||
})) << "Timeout, topic update not received, i=" << i;
|
||||
|
||||
// Check data
|
||||
EXPECT_FLOAT_EQ(latest_data.r, 1.f);
|
||||
EXPECT_FLOAT_EQ(latest_data.g, (float) i);
|
||||
EXPECT_FLOAT_EQ(latest_data.b, 2.f);
|
||||
}
|
||||
}
|
||||
|
||||
// Now check the translation into the other direction
|
||||
{
|
||||
int num_topic_updates = 0;
|
||||
DirectTranslationTest::MessageOlder latest_data{};
|
||||
auto publisher = _app_node->create_publisher<DirectTranslationTest::MessageNewer>(topic_name_newer_version,
|
||||
rclcpp::QoS(1).best_effort());
|
||||
auto subscriber = _app_node->create_subscription<DirectTranslationTest::MessageOlder>(topic_name_older_version,
|
||||
rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data, this](
|
||||
DirectTranslationTest::MessageOlder::UniquePtr msg) -> void {
|
||||
RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated: %.3f", (double) msg->data);
|
||||
latest_data = *msg;
|
||||
++num_topic_updates;
|
||||
});
|
||||
|
||||
monitor.updateNow();
|
||||
|
||||
// Wait until there is a subscriber & publisher
|
||||
ASSERT_TRUE(spinWithTimeout([&subscriber, &publisher]() {
|
||||
return subscriber->get_publisher_count() > 0 && publisher->get_subscription_count() > 0;
|
||||
})) << "Timeout, no publisher/subscriber found";
|
||||
|
||||
// Publish some data & wait for it to arrive
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
DirectTranslationTest::MessageNewer msg_newer;
|
||||
msg_newer.r = (float)i;
|
||||
msg_newer.g = (float)i * 10.f;
|
||||
msg_newer.b = (float)i * 100.f;
|
||||
publisher->publish(msg_newer);
|
||||
|
||||
ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() {
|
||||
return num_topic_updates == i + 1;
|
||||
})) << "Timeout, topic update not received, i=" << i;
|
||||
|
||||
// Check data
|
||||
EXPECT_FLOAT_EQ(latest_data.data, 111.f * (float)i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV1, std_msgs::msg::Float32, 1u);
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV1, std_msgs::msg::Float64, 1u);
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeCV1, std_msgs::msg::Int64, 1u);
|
||||
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV2, std_msgs::msg::ColorRGBA, 2u);
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV2, std_msgs::msg::Int64, 2u);
|
||||
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV3, std_msgs::msg::Float64, 3u);
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV3, std_msgs::msg::Int64, 3u);
|
||||
DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeCV3, std_msgs::msg::Float32, 3u);
|
||||
|
||||
class TranslationMultiTestV2 {
|
||||
public:
|
||||
using MessagesOlder = TypesArray<MessageTypeAV1, MessageTypeBV1, MessageTypeCV1>;
|
||||
static constexpr const char* kTopicsOlder[] = {
|
||||
"test/multi_translation_topic_a",
|
||||
"test/multi_translation_topic_b",
|
||||
"test/multi_translation_topic_c",
|
||||
};
|
||||
static_assert(MessageTypeAV1::MESSAGE_VERSION == 1);
|
||||
static_assert(MessageTypeBV1::MESSAGE_VERSION == 1);
|
||||
static_assert(MessageTypeCV1::MESSAGE_VERSION == 1);
|
||||
|
||||
using MessagesNewer = TypesArray<MessageTypeAV2, MessageTypeBV2>;
|
||||
static constexpr const char* kTopicsNewer[] = {
|
||||
"test/multi_translation_topic_a",
|
||||
"test/multi_translation_topic_b",
|
||||
};
|
||||
static_assert(MessageTypeAV2::MESSAGE_VERSION == 2);
|
||||
static_assert(MessageTypeBV2::MESSAGE_VERSION == 2);
|
||||
|
||||
static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2,
|
||||
const MessagesOlder::Type3 &msg_older3,
|
||||
MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2) {
|
||||
msg_newer1.r = msg_older1.data;
|
||||
msg_newer1.g = (float)msg_older2.data;
|
||||
msg_newer1.b = (float)msg_older3.data;
|
||||
msg_newer2.data = msg_older3.data * 10;
|
||||
}
|
||||
static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2,
|
||||
MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2, MessagesOlder::Type3 &msg_older3) {
|
||||
msg_older1.data = msg_newer1.r;
|
||||
msg_older2.data = msg_newer1.g;
|
||||
msg_older3.data = msg_newer2.data / 10;
|
||||
}
|
||||
};
|
||||
|
||||
class TranslationMultiTestV3 {
|
||||
public:
|
||||
using MessagesOlder = TypesArray<MessageTypeAV2, MessageTypeBV2>;
|
||||
static constexpr const char* kTopicsOlder[] = {
|
||||
"test/multi_translation_topic_a",
|
||||
"test/multi_translation_topic_b",
|
||||
};
|
||||
|
||||
using MessagesNewer = TypesArray<MessageTypeAV3, MessageTypeBV3, MessageTypeCV3>;
|
||||
static constexpr const char* kTopicsNewer[] = {
|
||||
"test/multi_translation_topic_a",
|
||||
"test/multi_translation_topic_b",
|
||||
"test/multi_translation_topic_c",
|
||||
};
|
||||
|
||||
static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2,
|
||||
MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2, MessagesNewer::Type3 &msg_newer3) {
|
||||
msg_newer1.data = msg_older1.r;
|
||||
msg_newer2.data = (int64_t)msg_older1.g;
|
||||
msg_newer3.data = (float)msg_older2.data + 100;
|
||||
}
|
||||
static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, const MessagesNewer::Type3 &msg_newer3,
|
||||
MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2) {
|
||||
msg_older1.r = (float)msg_newer1.data;
|
||||
msg_older1.g = (float)msg_newer2.data;
|
||||
msg_older2.data = (int64_t)msg_newer3.data - 100;
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(PubSubGraphTest, TranslationMulti) {
|
||||
RegisteredTranslationsTest registered_translations;
|
||||
// Register 3 different message versions, with 3 types -> 2 types -> 3 types
|
||||
registered_translations.registerTranslation<TranslationMultiTestV2>();
|
||||
registered_translations.registerTranslation<TranslationMultiTestV3>();
|
||||
|
||||
PubSubGraph graph(*_test_node, registered_translations.topicTranslations());
|
||||
Monitor monitor(*_test_node, &graph, nullptr);
|
||||
|
||||
const std::string topic_name_a = TranslationMultiTestV2::kTopicsOlder[0];
|
||||
const std::string topic_name_b = TranslationMultiTestV2::kTopicsOlder[1];
|
||||
const std::string topic_name_c = TranslationMultiTestV2::kTopicsOlder[2];
|
||||
|
||||
// Create publishers for version 1 + subscribers for version 3
|
||||
int num_topic_updates = 0;
|
||||
MessageTypeAV3 latest_data_a{};
|
||||
MessageTypeBV3 latest_data_b{};
|
||||
MessageTypeCV3 latest_data_c{};
|
||||
auto publisher_a = _app_node->create_publisher<MessageTypeAV1>(getVersionedTopicName(topic_name_a, MessageTypeAV1::MESSAGE_VERSION),
|
||||
rclcpp::QoS(1).best_effort());
|
||||
auto publisher_b = _app_node->create_publisher<MessageTypeBV1>(getVersionedTopicName(topic_name_b, MessageTypeBV1::MESSAGE_VERSION),
|
||||
rclcpp::QoS(1).best_effort());
|
||||
auto publisher_c = _app_node->create_publisher<MessageTypeCV1>(getVersionedTopicName(topic_name_c, MessageTypeCV1::MESSAGE_VERSION),
|
||||
rclcpp::QoS(1).best_effort());
|
||||
auto subscriber_a = _app_node->create_subscription<MessageTypeAV3>(getVersionedTopicName(topic_name_a, MessageTypeAV3::MESSAGE_VERSION),
|
||||
rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_a, this](
|
||||
MessageTypeAV3::UniquePtr msg) -> void {
|
||||
RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (A): %.3f", (double) msg->data);
|
||||
latest_data_a = *msg;
|
||||
++num_topic_updates;
|
||||
});
|
||||
auto subscriber_b = _app_node->create_subscription<MessageTypeBV3>(getVersionedTopicName(topic_name_b, MessageTypeBV3::MESSAGE_VERSION),
|
||||
rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_b, this](
|
||||
MessageTypeBV3::UniquePtr msg) -> void {
|
||||
RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (B): %.3f", (double) msg->data);
|
||||
latest_data_b = *msg;
|
||||
++num_topic_updates;
|
||||
});
|
||||
auto subscriber_c = _app_node->create_subscription<MessageTypeCV3>(getVersionedTopicName(topic_name_c, MessageTypeCV3::MESSAGE_VERSION),
|
||||
rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_c, this](
|
||||
MessageTypeCV3::UniquePtr msg) -> void {
|
||||
RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (C): %.3f", (double) msg->data);
|
||||
latest_data_c = *msg;
|
||||
++num_topic_updates;
|
||||
});
|
||||
|
||||
monitor.updateNow();
|
||||
|
||||
// Wait until there is a subscriber & publisher
|
||||
ASSERT_TRUE(spinWithTimeout([&]() {
|
||||
return subscriber_a->get_publisher_count() > 0 && subscriber_b->get_publisher_count() > 0 && subscriber_c->get_publisher_count() > 0 &&
|
||||
publisher_a->get_subscription_count() > 0 && publisher_b->get_subscription_count() > 0 && publisher_c->get_subscription_count() > 0;
|
||||
})) << "Timeout, no publisher/subscriber found";
|
||||
|
||||
// Publish some data & wait for it to arrive
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
MessageTypeAV1 msg_older_a;
|
||||
msg_older_a.data = (float) i;
|
||||
publisher_a->publish(msg_older_a);
|
||||
|
||||
MessageTypeBV1 msg_older_b;
|
||||
msg_older_b.data = (float) i * 10.f;
|
||||
publisher_b->publish(msg_older_b);
|
||||
|
||||
MessageTypeCV1 msg_older_c;
|
||||
msg_older_c.data = i * 100;
|
||||
publisher_c->publish(msg_older_c);
|
||||
|
||||
ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() {
|
||||
return num_topic_updates == (i + 1) * 3;
|
||||
})) << "Timeout, topic update not received, i=" << i << ", num updates=" << num_topic_updates;
|
||||
|
||||
// Check data
|
||||
EXPECT_FLOAT_EQ(latest_data_a.data, (float)i);
|
||||
EXPECT_FLOAT_EQ(latest_data_b.data, (float)i * 10.f);
|
||||
EXPECT_FLOAT_EQ(latest_data_c.data, ((float)i * 100.f) * 10.f + 100.f);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,215 @@
|
||||
/****************************************************************************
|
||||
* Copyright (c) 2024 PX4 Development Team.
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <src/monitor.h>
|
||||
#include <src/service_graph.h>
|
||||
#include <src/translation_util.h>
|
||||
|
||||
#include <translation_node/srv/test_v0.hpp>
|
||||
#include <translation_node/srv/test_v1.hpp>
|
||||
#include <translation_node/srv/test_v2.hpp>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
|
||||
class ServiceTest : public testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
_test_node = std::make_shared<rclcpp::Node>("test_node");
|
||||
_app_node = std::make_shared<rclcpp::Node>("app_node");
|
||||
_executor.add_node(_test_node);
|
||||
_executor.add_node(_app_node);
|
||||
|
||||
for (auto& node : {_app_node, _test_node}) {
|
||||
auto ret = rcutils_logging_set_logger_level(
|
||||
node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
|
||||
if (ret != RCUTILS_RET_OK) {
|
||||
RCLCPP_ERROR(
|
||||
node->get_logger(), "Error setting severity: %s",
|
||||
rcutils_get_error_string().str);
|
||||
rcutils_reset_error();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool spinWithTimeout(const std::function<bool(void)>& predicate) {
|
||||
const auto start = _app_node->now();
|
||||
while (_app_node->now() - start < 5s) {
|
||||
_executor.spin_some();
|
||||
if (predicate()) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp::Node> _test_node;
|
||||
std::shared_ptr<rclcpp::Node> _app_node;
|
||||
rclcpp::executors::SingleThreadedExecutor _executor;
|
||||
};
|
||||
|
||||
class RegisteredTranslationsTest : public RegisteredTranslations {
|
||||
public:
|
||||
RegisteredTranslationsTest() = default;
|
||||
};
|
||||
|
||||
|
||||
class ServiceTestV0V1 {
|
||||
public:
|
||||
using MessageOlder = translation_node::srv::TestV0;
|
||||
using MessageNewer = translation_node::srv::TestV1;
|
||||
|
||||
static constexpr const char* kTopic = "test/service";
|
||||
|
||||
static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) {
|
||||
msg_newer.request_a = msg_older.request_a;
|
||||
}
|
||||
|
||||
static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) {
|
||||
msg_older.request_a = msg_newer.request_a;
|
||||
}
|
||||
|
||||
static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) {
|
||||
msg_newer.response_a = msg_older.response_a;
|
||||
}
|
||||
|
||||
static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) {
|
||||
msg_older.response_a = msg_newer.response_a;
|
||||
}
|
||||
};
|
||||
|
||||
class ServiceTestV1V2 {
|
||||
public:
|
||||
using MessageOlder = translation_node::srv::TestV1;
|
||||
using MessageNewer = translation_node::srv::TestV2;
|
||||
|
||||
static constexpr const char* kTopic = "test/service";
|
||||
|
||||
static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) {
|
||||
msg_newer.request_a = msg_older.request_a;
|
||||
msg_newer.request_b = 1234;
|
||||
}
|
||||
|
||||
static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) {
|
||||
msg_older.request_a = msg_newer.request_a + msg_newer.request_b;
|
||||
}
|
||||
|
||||
static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) {
|
||||
msg_newer.response_a = msg_older.response_a;
|
||||
msg_newer.response_b = 32;
|
||||
}
|
||||
|
||||
static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) {
|
||||
msg_older.response_a = msg_newer.response_a + msg_newer.response_b;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
TEST_F(ServiceTest, Test)
|
||||
{
|
||||
RegisteredTranslationsTest registered_translations;
|
||||
registered_translations.registerServiceDirectTranslation<ServiceTestV0V1>();
|
||||
registered_translations.registerServiceDirectTranslation<ServiceTestV1V2>();
|
||||
|
||||
ServiceGraph graph(*_test_node, registered_translations.serviceTranslations());
|
||||
Monitor monitor(*_test_node, nullptr, &graph);
|
||||
|
||||
const std::string topic_name = ServiceTestV1V2::kTopic;
|
||||
const std::string topic_name_v0 = getVersionedTopicName(topic_name, ServiceTestV0V1::MessageOlder::Request::MESSAGE_VERSION);
|
||||
const std::string topic_name_v1 = getVersionedTopicName(topic_name, ServiceTestV0V1::MessageNewer::Request::MESSAGE_VERSION);
|
||||
const std::string topic_name_v2 = getVersionedTopicName(topic_name, ServiceTestV1V2::MessageNewer::Request::MESSAGE_VERSION);
|
||||
|
||||
|
||||
// Create service + clients
|
||||
int num_service_requests = 0;
|
||||
auto service = _app_node->create_service<ServiceTestV0V1::MessageOlder>(topic_name_v0, [&num_service_requests](
|
||||
const ServiceTestV0V1::MessageOlder::Request::SharedPtr request, ServiceTestV0V1::MessageOlder::Response::SharedPtr response) {
|
||||
response->response_a = request->request_a + 1;
|
||||
++num_service_requests;
|
||||
});
|
||||
auto client0 = _app_node->create_client<ServiceTestV0V1::MessageOlder>(topic_name_v0);
|
||||
auto client1 = _app_node->create_client<ServiceTestV0V1::MessageNewer>(topic_name_v1);
|
||||
auto client2 = _app_node->create_client<ServiceTestV1V2::MessageNewer>(topic_name_v2);
|
||||
|
||||
monitor.updateNow();
|
||||
|
||||
// Wait until there is a service for each client
|
||||
ASSERT_TRUE(spinWithTimeout([&client0, &client1, &client2]() {
|
||||
return client0->service_is_ready() && client1->service_is_ready() && client2->service_is_ready();
|
||||
})) << "Timeout, no service for clients found: " << client0->service_is_ready() << client1->service_is_ready() << client2->service_is_ready();
|
||||
|
||||
|
||||
|
||||
// Make some requests
|
||||
int expected_num_service_requests = 1;
|
||||
|
||||
// Client 1
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
auto request = std::make_shared<ServiceTestV0V1::MessageNewer::Request>();
|
||||
ServiceTestV0V1::MessageNewer::Response response;
|
||||
request->request_a = i;
|
||||
bool got_response = false;
|
||||
client1->async_send_request(request, [&got_response, &response](rclcpp::Client<ServiceTestV0V1::MessageNewer>::SharedFuture result) {
|
||||
got_response = true;
|
||||
response = *result.get();
|
||||
});
|
||||
|
||||
ASSERT_TRUE(spinWithTimeout([&got_response]() {
|
||||
return got_response;
|
||||
})) << "Timeout, reply not received, i=" << i;
|
||||
|
||||
// Check data
|
||||
EXPECT_EQ(response.response_a, i + 1);
|
||||
EXPECT_EQ(num_service_requests, expected_num_service_requests);
|
||||
++expected_num_service_requests;
|
||||
}
|
||||
|
||||
// Client 0
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
auto request = std::make_shared<ServiceTestV0V1::MessageOlder::Request>();
|
||||
ServiceTestV0V1::MessageOlder::Response response;
|
||||
request->request_a = i * 10;
|
||||
bool got_response = false;
|
||||
client0->async_send_request(request, [&got_response, &response](rclcpp::Client<ServiceTestV0V1::MessageOlder>::SharedFuture result) {
|
||||
got_response = true;
|
||||
response = *result.get();
|
||||
});
|
||||
|
||||
ASSERT_TRUE(spinWithTimeout([&got_response]() {
|
||||
return got_response;
|
||||
})) << "Timeout, reply not received, i=" << i;
|
||||
|
||||
// Check data
|
||||
EXPECT_EQ(response.response_a, i * 10 + 1);
|
||||
EXPECT_EQ(num_service_requests, expected_num_service_requests);
|
||||
++expected_num_service_requests;
|
||||
}
|
||||
|
||||
// Client 2
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
auto request = std::make_shared<ServiceTestV1V2::MessageNewer::Request>();
|
||||
ServiceTestV1V2::MessageNewer::Response response;
|
||||
request->request_a = i * 10;
|
||||
request->request_b = i;
|
||||
bool got_response = false;
|
||||
client2->async_send_request(request, [&got_response, &response](rclcpp::Client<ServiceTestV1V2::MessageNewer>::SharedFuture result) {
|
||||
got_response = true;
|
||||
response = *result.get();
|
||||
});
|
||||
|
||||
ASSERT_TRUE(spinWithTimeout([&got_response]() {
|
||||
return got_response;
|
||||
})) << "Timeout, reply not received, i=" << i;
|
||||
|
||||
// Check data
|
||||
EXPECT_EQ(response.response_a, i + i * 10 + 1);
|
||||
EXPECT_EQ(response.response_b, 32);
|
||||
EXPECT_EQ(num_service_requests, expected_num_service_requests);
|
||||
++expected_num_service_requests;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,4 @@
|
||||
uint32 MESSAGE_VERSION = 0
|
||||
uint8 request_a
|
||||
---
|
||||
uint64 response_a
|
||||
@@ -0,0 +1,4 @@
|
||||
uint32 MESSAGE_VERSION = 1
|
||||
uint64 request_a
|
||||
---
|
||||
uint8 response_a
|
||||
@@ -0,0 +1,6 @@
|
||||
uint32 MESSAGE_VERSION = 2
|
||||
uint8 request_a
|
||||
uint64 request_b
|
||||
---
|
||||
uint16 response_a
|
||||
uint64 response_b
|
||||
Reference in New Issue
Block a user