Skip to content

Commit

Permalink
Added more test for bridge
Browse files Browse the repository at this point in the history 
Signed-off-by: Alberto Tudela <[email protected]>
  • Loading branch information
@ajtudela
ajtudela committed Nov 13, 2024
1 parent 3c6743e commit 4b27ca4
Show file tree
Hide file tree
Showing 5 changed files with 272 additions and 134 deletions.
44 changes: 15 additions & 29 deletions dsr_bridge/include/dsr_bridge/dsr_bridge.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,30 +35,6 @@
namespace dsr_bridge
{

// Struct to store edges when nodes are not created yet
struct LostEdge
{
LostEdge(
std::string parent, std::string child, std::string edge_type,
std::vector<std::string> & attributes)
{
from = parent;
to = child;
type = edge_type;
attrs = attributes;
}

bool operator==(const LostEdge & other_edge) const
{
return (from == other_edge.from) && (to == other_edge.to) && (type == other_edge.type);
}

std::string from;
std::string to;
std::string type;
std::vector<std::string> attrs;
};

/**
* @class dsr_bridge::DSRBridge
* @brief Bridge to connect the DSR graphs between machines throughROS 2 topics.
Expand Down Expand Up @@ -166,11 +142,22 @@ class DSRBridge : public dsr_util::AgentNode
*/
dsr_msgs::msg::Node to_msg(const DSR::Node & node, bool deleted = false);

std::optional<DSR::Edge> create_dsr_edge(
std::string from, std::string to, const std::string & type, std::vector<std::string> & atts);
/**
* @brief Create a DSR::Edge from a ROS 2 message.
*
* @param msg The ROS 2 message.
* @return DSR::Edge The DSR Edge created.
*/
DSR::Edge from_msg(const dsr_msgs::msg::Edge & msg);

dsr_msgs::msg::Edge create_msg_edge(
std::uint64_t from, std::uint64_t to, const std::string & type);
/**
* @brief Create a dsr_msgs::msg::Edge from a DSR::Edge.
*
* @param edge The DSR edge.
* @param deleted If the edge has to be marked as deleted.
* @return dsr_msgs::msg::Edge The ROS 2 message.
*/
dsr_msgs::msg::Edge to_msg(const DSR::Edge & edge, bool deleted = false);

/**
* @brief Modify the attributes of a DSR element with the given attributes in a vector of strings.
Expand All @@ -191,7 +178,6 @@ class DSRBridge : public dsr_util::AgentNode
rclcpp::Publisher<dsr_msgs::msg::Edge>::SharedPtr edge_to_ros_pub_;
rclcpp::Publisher<dsr_msgs::msg::Node>::SharedPtr node_to_ros_pub_;
std::string edge_topic_, node_topic_;
std::vector<LostEdge> lost_edges_;
};

} // namespace dsr_bridge
Expand Down
139 changes: 34 additions & 105 deletions dsr_bridge/src/dsr_bridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,19 +79,11 @@ void DSRBridge::edge_from_ros_callback(const dsr_msgs::msg::Edge::SharedPtr msg)
}
// Create the current edge
if (!msg->deleted && !msg->updated) {
auto new_edge = create_dsr_edge(msg->parent, msg->child, msg->type, msg->attributes);
if (new_edge.has_value()) {
modify_attributes(new_edge.value(), msg->attributes);
if (G_->insert_or_assign_edge(new_edge.value())) {
RCLCPP_DEBUG(
this->get_logger(),
"Inserted edge [%s->%s] of type [%s] in the DSR",
msg->parent.c_str(), msg->child.c_str(), msg->type.c_str());
}
} else {
RCLCPP_WARN(
auto new_edge = from_msg(*msg);
if (G_->insert_or_assign_edge(new_edge)) {
RCLCPP_DEBUG(
this->get_logger(),
"The edge [%s->%s] of type [%s] could not be inserted in the DSR",
"Inserted edge [%s->%s] of type [%s] in the DSR",
msg->parent.c_str(), msg->child.c_str(), msg->type.c_str());
}
} else if (!msg->deleted && msg->updated) {
Expand All @@ -111,29 +103,8 @@ void DSRBridge::edge_from_ros_callback(const dsr_msgs::msg::Edge::SharedPtr msg)
}
}
} else if (msg->deleted) {
// Delete the current edge
if (auto edge = G_->get_edge(msg->parent, msg->child, msg->type); edge.has_value()) {
if (G_->delete_edge(msg->parent, msg->child, msg->type)) {
RCLCPP_INFO(
this->get_logger(),
"Deleted edge [%s->%s] successfully of type [%s]",
msg->parent.c_str(), msg->child.c_str(), msg->type.c_str());
} else {
RCLCPP_WARN(
this->get_logger(),
"The edge [%s->%s] of type [%s] couldn't be deleted",
msg->parent.c_str(), msg->child.c_str(), msg->type.c_str());
}
} else {
LostEdge lost(msg->parent, msg->child, msg->type, msg->attributes);
auto it = std::find(lost_edges_.begin(), lost_edges_.end(), lost);
if (it != lost_edges_.end()) {
lost_edges_.erase(it);
RCLCPP_INFO(this->get_logger(), "Deleted edge from lost_edges vector");
}
}
delete_edge(msg->parent, msg->child, msg->type);
} else {
// Error
RCLCPP_ERROR(this->get_logger(), "The edge message is not well defined");
}
}
Expand Down Expand Up @@ -199,37 +170,6 @@ void DSRBridge::node_created(std::uint64_t id, const std::string & type)
dsr_node.value().name().c_str(), type.c_str());
}
}
// Retry to insert lost edges
std::vector<std::vector<LostEdge>::iterator> delete_lost_edges;
unsigned int i = 0;
for (auto & edge : lost_edges_) {
auto parent_node = G_->get_node(edge.from);
auto child_node = G_->get_node(edge.to);
if (parent_node.has_value() && child_node.has_value()) {
DSR::Edge new_edge;
new_edge.from(parent_node.value().id());
new_edge.to(child_node.value().id());
new_edge.type(edge.type);
modify_attributes(new_edge, edge.attrs);
if (G_->insert_or_assign_edge(new_edge)) {
RCLCPP_DEBUG(
this->get_logger(),
"Inserted edge losted [%s->%s] of type [%s] in the DSR",
edge.from.c_str(), edge.to.c_str(), edge.type.c_str());
auto it = (lost_edges_.begin() + i);
delete_lost_edges.push_back(it);
} else {
RCLCPP_WARN(
this->get_logger(),
"The edge losted [%s->%s] of type [%s] could not be inserted in the DSR",
edge.from.c_str(), edge.to.c_str(), edge.type.c_str());
}
}
i++;
}
for (auto it : delete_lost_edges) {
lost_edges_.erase(it);
}
}

void DSRBridge::node_attr_updated(uint64_t id, const std::vector<std::string> & /*att_names*/)
Expand Down Expand Up @@ -259,35 +199,28 @@ void DSRBridge::edge_updated(std::uint64_t from, std::uint64_t to, const std::st
if (auto source = G_->get_attrib_by_name<source_att>(dsr_edge.value());
(source.has_value() && source.value() == source_))
{
// Create the message
auto edge_msg = create_msg_edge(from, to, type);
// Get all the attributes
edge_msg.attributes = dsr_util::helpers::attributes_to_string(dsr_edge.value().attrs());
// Publish the message
edge_to_ros_pub_->publish(edge_msg);
RCLCPP_DEBUG(
this->get_logger(),
"The new edge [%s->%s] of type [%s] has been published through ROS",
edge_msg.parent.c_str(), edge_msg.child.c_str(), edge_msg.type.c_str());
edge_to_ros_pub_->publish(to_msg(dsr_edge.value()));
}
}
}

void DSRBridge::edge_attr_updated(
std::uint64_t from, std::uint64_t to,
const std::string & type, const std::vector<std::string> & att_names)
const std::string & type, const std::vector<std::string> & /*att_names*/)
{
// TODO(ajtudela): Replace this with new attr topic
// Filter the edges that comes from the same source
if (auto dsr_edge = G_->get_edge(from, to, type); dsr_edge.has_value()) {
if (auto source = G_->get_attrib_by_name<source_att>(dsr_edge.value());
(source.has_value() && source.value() == source_))
{
// Create the message
auto edge_msg = create_msg_edge(from, to, type);
auto edge_msg = to_msg(dsr_edge.value());
// Mark the node as updated
edge_msg.updated = true;
// Get all the updated attributes
edge_msg.attributes = attributes_updated_to_string(dsr_edge.value(), att_names);
// edge_msg.attributes = attributes_updated_to_string(dsr_edge.value(), att_names);
// Publish the message
edge_to_ros_pub_->publish(edge_msg);
RCLCPP_DEBUG(
Expand All @@ -310,16 +243,15 @@ void DSRBridge::node_deleted_by_node(const DSR::Node & node)

void DSRBridge::edge_deleted(std::uint64_t from, std::uint64_t to, const std::string & edge_tag)
{
// Create the message
auto edge_msg = create_msg_edge(from, to, edge_tag);
// Mark the edge as deleted
edge_msg.deleted = true;
// Publish the message
edge_to_ros_pub_->publish(edge_msg);
RCLCPP_DEBUG(
this->get_logger(),
"The deleted edge [%s->%s] of type [%s] has been published through ROS",
edge_msg.parent.c_str(), edge_msg.child.c_str(), edge_msg.type.c_str());
if (auto dsr_edge = G_->get_edge(from, to, edge_tag); dsr_edge.has_value()) {
auto edge_msg = to_msg(dsr_edge.value(), true);
edge_to_ros_pub_->publish(edge_msg);
RCLCPP_DEBUG(
this->get_logger(),
"The deleted edge [%s->%s] of type [%s] has been published through ROS",
edge_msg.parent.c_str(), edge_msg.child.c_str(), edge_msg.type.c_str());
}
}

DSR::Node DSRBridge::from_msg(const dsr_msgs::msg::Node & msg)
Expand Down Expand Up @@ -348,40 +280,38 @@ dsr_msgs::msg::Node DSRBridge::to_msg(const DSR::Node & node, bool deleted)
return node_msg;
}

std::optional<DSR::Edge> DSRBridge::create_dsr_edge(
std::string from, std::string to, const std::string & type, std::vector<std::string> & atts)
DSR::Edge DSRBridge::from_msg(const dsr_msgs::msg::Edge & msg)
{
if (!edge_types::check_type(type)) {
throw std::runtime_error("Error, [" + type + "] is not a valid edge type");
if (!edge_types::check_type(msg.type)) {
throw std::runtime_error("Error, [" + msg.type + "] is not a valid edge type");
}

DSR::Edge new_edge;
auto parent_node = G_->get_node(from);
auto child_node = G_->get_node(to);
auto parent_node = G_->get_node(msg.parent);
auto child_node = G_->get_node(msg.child);
if (parent_node.has_value() && child_node.has_value()) {
new_edge.from(parent_node.value().id());
new_edge.to(child_node.value().id());
new_edge.type(type);
return new_edge;
new_edge.type(msg.type);
modify_attributes(new_edge, msg.attributes);
}

lost_edges_.push_back(LostEdge(from, to, type, atts));
return {};
return new_edge;
}

dsr_msgs::msg::Edge DSRBridge::create_msg_edge(
std::uint64_t from, std::uint64_t to, const std::string & type)
dsr_msgs::msg::Edge DSRBridge::to_msg(const DSR::Edge & edge, bool deleted)
{
dsr_msgs::msg::Edge edge_msg;
auto parent_node = G_->get_node(from);
auto child_node = G_->get_node(to);
auto parent_node = G_->get_node(edge.from());
auto child_node = G_->get_node(edge.to());
if (parent_node.has_value() && child_node.has_value()) {
edge_msg.header.stamp = this->now();
edge_msg.header.frame_id = source_;
edge_msg.parent = parent_node.value().name();
edge_msg.child = child_node.value().name();
edge_msg.type = type;
edge_msg.type = edge.type();
edge_msg.attributes = dsr_util::helpers::attributes_to_string(edge.attrs());
edge_msg.updated = false;
edge_msg.deleted = false;
edge_msg.deleted = deleted;
}
return edge_msg;
}
Expand All @@ -407,8 +337,7 @@ void DSRBridge::modify_attributes(TYPE & elem, const std::vector<std::string> &

template<typename TYPE>
std::vector<std::string> DSRBridge::attributes_updated_to_string(
TYPE & elem,
const std::vector<std::string> & atts)
TYPE & elem, const std::vector<std::string> & atts)
{
std::vector<std::string> att_vector_str;
for (const auto & att_name : atts) {
Expand Down
8 changes: 8 additions & 0 deletions dsr_bridge/test/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,11 @@
# Test for test_bridge_integration
ament_add_gtest(test_bridge_integration test_bridge_integration.cpp)
target_link_libraries(test_bridge_integration
${fastrtps_LIBRARIES}
dsr_bridge
ament_index_cpp::ament_index_cpp
)

# Test for test_bridge
ament_add_gtest(test_bridge test_bridge.cpp)
target_link_libraries(test_bridge
Expand Down
75 changes: 75 additions & 0 deletions dsr_bridge/test/test_bridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,18 @@ class DSRBridgeFixture : public dsr_bridge::DSRBridge
return G_;
}

template<typename node_type>
std::optional<DSR::Node> add_node(const std::string & name)
{
return dsr_bridge::DSRBridge::add_node<node_type>(name);
}

template<typename edge_type>
std::optional<DSR::Edge> add_edge(const std::string & parent, const std::string & child)
{
return dsr_bridge::DSRBridge::add_edge<edge_type>(parent, child);
}

DSR::Node from_msg(const dsr_msgs::msg::Node & msg)
{
return dsr_bridge::DSRBridge::from_msg(msg);
Expand All @@ -42,6 +54,16 @@ class DSRBridgeFixture : public dsr_bridge::DSRBridge
{
return dsr_bridge::DSRBridge::to_msg(node, deleted);
}

DSR::Edge from_msg(const dsr_msgs::msg::Edge & msg)
{
return dsr_bridge::DSRBridge::from_msg(msg);
}

dsr_msgs::msg::Edge to_msg(const DSR::Edge & edge, bool deleted)
{
return dsr_bridge::DSRBridge::to_msg(edge, deleted);
}
};

TEST_F(DsrUtilTest, DSRBridgeConfigure) {
Expand Down Expand Up @@ -101,6 +123,59 @@ TEST_F(DsrUtilTest, DSRBridgeCreateMsgNode) {
EXPECT_TRUE(node_msg.deleted);
}

TEST_F(DsrUtilTest, DSRBridgeCreateDSREdge) {
// Create the node
auto agent_node = std::make_shared<DSRBridgeFixture>();
agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
agent_node->configure();
agent_node->activate();

// Add the DRS nodes
auto dsr_parent_node = agent_node->add_node<robot_node_type>("robot_parent");
auto dsr_child_node = agent_node->add_node<robot_node_type>("robot_child");

// Create the message
dsr_msgs::msg::Edge edge_msg;
edge_msg.parent = "robot_parent";
edge_msg.child = "robot_child";
edge_msg.type = "is";
edge_msg.attributes = {"source", "robot", "0"};

// Create the DSR edge
auto dsr_edge = agent_node->from_msg(edge_msg);
EXPECT_EQ(dsr_edge.from(), agent_node->get_graph()->get_node("robot_parent").value().id());
EXPECT_EQ(dsr_edge.to(), agent_node->get_graph()->get_node("robot_child").value().id());
EXPECT_EQ(dsr_edge.type(), "is");
auto attributes = dsr_edge.attrs();
auto search = attributes.find("source");
EXPECT_TRUE(search != attributes.end());
EXPECT_EQ(std::get<std::string>(search->second.value()), "robot");
}

TEST_F(DsrUtilTest, DSRBridgeCreateMsgEdge) {
// Create the node
auto agent_node = std::make_shared<DSRBridgeFixture>();
agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
agent_node->configure();
agent_node->activate();

// Add the DRS nodes and edge
auto dsr_parent_node = agent_node->add_node<robot_node_type>("robot_parent");
auto dsr_child_node = agent_node->add_node<robot_node_type>("robot_child");
auto dsr_edge = agent_node->add_edge<is_edge_type>("robot_parent", "robot_child");

// Create the message
auto edge_msg = agent_node->to_msg(dsr_edge.value(), true);
EXPECT_EQ(edge_msg.parent, "robot_parent");
EXPECT_EQ(edge_msg.child, "robot_child");
EXPECT_EQ(edge_msg.type, "is");
auto attributes = dsr_edge.value().attrs();
auto search = attributes.find("source");
EXPECT_TRUE(search != attributes.end());
EXPECT_EQ(std::get<std::string>(search->second.value()), "robot");
EXPECT_TRUE(edge_msg.deleted);
}

int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
Expand Down
Loading

0 comments on commit 4b27ca4

Please sign in to comment.