diff --git a/dsr_bridge/CMakeLists.txt b/dsr_bridge/CMakeLists.txt index c5a16a8..bd3678f 100644 --- a/dsr_bridge/CMakeLists.txt +++ b/dsr_bridge/CMakeLists.txt @@ -57,8 +57,7 @@ find_package(rclcpp_components REQUIRED) # ########## # # Build ## # ########## -# Set DSR and FastRTPS -set(dsr_LIBRARIES dsr_api dsr_core dsr_gui) +# Set FastRTPS libraries set(fastrtps_LIBRARIES fastrtps fastcdr) # Add library diff --git a/dsr_bridge/include/dsr_bridge/dsr_bridge.hpp b/dsr_bridge/include/dsr_bridge/dsr_bridge.hpp index f89ffbd..c10ad57 100644 --- a/dsr_bridge/include/dsr_bridge/dsr_bridge.hpp +++ b/dsr_bridge/include/dsr_bridge/dsr_bridge.hpp @@ -81,41 +81,110 @@ class DSRBridge : public dsr_util::AgentNode */ CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; -private: - // ROS callbacks +protected: + /** + * @brief Callback executed when an edge is received from a ROS 2 topic. + * + * @param msg The edge received. + */ void edge_from_ros_callback(const dsr_msgs::msg::Edge::SharedPtr msg); + + /** + * @brief Callback executed when a node is received from a ROS 2 topic. + * + * @param msg The node received. + */ void node_from_ros_callback(const dsr_msgs::msg::Node::SharedPtr msg); - // DSR callbacks - void node_created(std::uint64_t id, const std::string & type)override; + /** + * @brief Callback executed when a node is created in the DSR graph. + * + * @param id The id of the node. + * @param type The type of the node. + */ + void node_created(std::uint64_t id, const std::string & type) override; + + /** + * @brief Callback executed when a node attribute is updated in the DSR graph. + * + * @param id The id of the node. + * @param att_names The names of the attributes updated. + */ void node_attr_updated(uint64_t id, const std::vector & att_names) override; + + /** + * @brief Callback executed when an edge is updated in the DSR graph. + * + * @param from The id of the parent node. + * @param to The id of the child node. + * @param type The type of the edge. + */ void edge_updated(std::uint64_t from, std::uint64_t to, const std::string & type) override; + + /** + * @brief Callback executed when an edge attribute is updated in the DSR graph. + * + * @param from The id of the parent node. + * @param to The id of the child node. + * @param type The type of the edge. + * @param att_names The names of the attributes updated. + */ void edge_attr_updated( std::uint64_t from, std::uint64_t to, const std::string & type, const std::vector & att_names) override; + + /** + * @brief Callback executed when a node is deleted in the DSR graph. + * + * @param id The id of the node. + */ void node_deleted_by_node(const DSR::Node & node)override; + + /** + * @brief Callback executed when an edge is deleted in the DSR graph. + * + * @param from The id of the parent node. + * @param to The id of the child node. + * @param edge_tag The type of the edge. + */ void edge_deleted(std::uint64_t from, std::uint64_t to, const std::string & edge_tag) override; - // Converter functions - std::optional create_dsr_node(std::string name, std::string type); + /** + * @brief Create a DSR::Node from a ROS 2 message. + * + * @param msg The ROS 2 message. + * @return DSR::Node The DSR node created. + */ + DSR::Node from_msg(const dsr_msgs::msg::Node & msg); + + /** + * @brief Create a dsr_msgs::msg::Node from a DSR::Node. + * + * @param node The DSR node. + * @param deleted If the node has to be marked as deleted. + * @return dsr_msgs::msg::Node The ROS 2 message. + */ + dsr_msgs::msg::Node to_msg(const DSR::Node & node, bool deleted = false); + std::optional create_dsr_edge( std::string from, std::string to, const std::string & type, std::vector & atts); - dsr_msgs::msg::Node create_msg_node(std::string name, std::string type); + dsr_msgs::msg::Edge create_msg_edge( std::uint64_t from, std::uint64_t to, const std::string & type); - // Helper functions + /** + * @brief Modify the attributes of a DSR element with the given attributes in a vector of strings. + * + * @tparam TYPE Type of the DSR element. + * @param elem The DSR element (node or edge) to modify. + * @param att_str The attributes to modify in format (name, value, type). + */ template - void modify_attributes(TYPE & elem, std::vector & att_str); + void modify_attributes(TYPE & elem, const std::vector & att_str); - std::string attribute_to_string(const DSR::Attribute & att); - std::vector attributes_to_string( - const std::map & atts); template std::vector attributes_updated_to_string( TYPE & elem, const std::vector & atts); - DSR::Attribute string_to_attribute(const std::string & att_value, int att_type); - std::string get_type_from_attribute(const DSR::Attribute & att); rclcpp::Subscription::SharedPtr edge_from_ros_sub_; rclcpp::Subscription::SharedPtr node_from_ros_sub_; diff --git a/dsr_bridge/src/dsr_bridge.cpp b/dsr_bridge/src/dsr_bridge.cpp index a01a6d6..6ae9513 100644 --- a/dsr_bridge/src/dsr_bridge.cpp +++ b/dsr_bridge/src/dsr_bridge.cpp @@ -19,11 +19,9 @@ #include #include -// ROS -#include "nav2_util/string_utils.hpp" - // DSR #include "dsr_util/qt_executor.hpp" +#include "dsr_util/helpers.hpp" #include "dsr_bridge/dsr_bridge.hpp" namespace dsr_bridge @@ -153,7 +151,7 @@ void DSRBridge::node_from_ros_callback(const dsr_msgs::msg::Node::SharedPtr msg) return; } - // Create or update the current node + // Create update or delete the current node if (!msg->deleted) { if (auto node = G_->get_node(msg->name); node.has_value()) { modify_attributes(node.value(), msg->attributes); @@ -168,9 +166,8 @@ void DSRBridge::node_from_ros_callback(const dsr_msgs::msg::Node::SharedPtr msg) "The node [%s] couldn't be updated in the DSR", msg->name.c_str()); } } else { - auto new_node = create_dsr_node(msg->name, msg->type); - modify_attributes(new_node.value(), msg->attributes); - if (auto id = G_->insert_node(new_node.value()); id.has_value()) { + auto new_node = from_msg(*msg); + if (auto id = G_->insert_node(new_node); id.has_value()) { RCLCPP_DEBUG( this->get_logger(), "Inserted [%s] node successfully of type [%s] in the DSR", @@ -181,46 +178,25 @@ void DSRBridge::node_from_ros_callback(const dsr_msgs::msg::Node::SharedPtr msg) "The node [%s] couldn't be inserted in the DSR", msg->name.c_str()); } } - } else if (msg->deleted) { - // Delete the node - if (auto node = G_->get_node(msg->name); node.has_value()) { - if (G_->delete_node(node.value().name())) { - RCLCPP_DEBUG( - this->get_logger(), - "Deleted [%s] node successfully of type [%s] in the DSR", - msg->name.c_str(), msg->type.c_str()); - } else { - RCLCPP_ERROR( - this->get_logger(), - "The node [%s] couldn't be deleted in the DSR", msg->name.c_str()); - } - - } else { - RCLCPP_ERROR( - this->get_logger(), - "The node [%s] could not be deleted in the DSR", msg->name.c_str()); - } + } else { + delete_node(msg->name); } } // DSR callbacks -void DSRBridge::node_created(std::uint64_t id, const std::string & /*type*/) +void DSRBridge::node_created(std::uint64_t id, const std::string & type) { // Filter the edges that comes from the same source if (auto dsr_node = G_->get_node(id); dsr_node.has_value()) { if (auto source = G_->get_attrib_by_name(dsr_node.value()); (source.has_value() && source == source_)) { - // Create the message - auto node_msg = create_msg_node(dsr_node.value().name(), dsr_node.value().type()); - // Get all the attributes - node_msg.attributes = attributes_to_string(dsr_node.value().attrs()); // Publish the message - node_to_ros_pub_->publish(node_msg); + node_to_ros_pub_->publish(to_msg(dsr_node.value())); RCLCPP_DEBUG( this->get_logger(), - "The new node [%s] of type [%s] has been published through ROS", - node_msg.name.c_str(), node_msg.type.c_str()); + "The node [%s] of type [%s] has been published through ROS", + dsr_node.value().name().c_str(), type.c_str()); } } // Retry to insert lost edges @@ -256,25 +232,22 @@ void DSRBridge::node_created(std::uint64_t id, const std::string & /*type*/) } } -void DSRBridge::node_attr_updated(uint64_t id, const std::vector & att_names) +void DSRBridge::node_attr_updated(uint64_t id, const std::vector & /*att_names*/) { + // TODO(ajtudela): Replace this with new attr topic // Filter the edges that comes from the same source if (auto dsr_node = G_->get_node(id); dsr_node.has_value()) { if (auto source = G_->get_attrib_by_name(dsr_node.value()); (source.has_value() && source.value() == source_)) { - // Create the message - auto node_msg = create_msg_node(dsr_node.value().name(), dsr_node.value().type()); - // Mark the node as updated - node_msg.updated = true; // Get all the updated attributes - node_msg.attributes = attributes_updated_to_string(dsr_node.value(), att_names); + // node_msg.attributes = attributes_updated_to_string(dsr_node.value(), att_names); // Publish the message - node_to_ros_pub_->publish(node_msg); + node_to_ros_pub_->publish(to_msg(dsr_node.value())); RCLCPP_DEBUG( this->get_logger(), - "The updated node [%s] of type [%s] has been published through ROS", - node_msg.name.c_str(), node_msg.type.c_str()); + "The node [%s] of type [%s] has been published through ROS", + dsr_node.value().name().c_str(), dsr_node.value().type().c_str()); } } } @@ -289,7 +262,7 @@ void DSRBridge::edge_updated(std::uint64_t from, std::uint64_t to, const std::st // Create the message auto edge_msg = create_msg_edge(from, to, type); // Get all the attributes - edge_msg.attributes = attributes_to_string(dsr_edge.value().attrs()); + 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( @@ -327,16 +300,12 @@ void DSRBridge::edge_attr_updated( void DSRBridge::node_deleted_by_node(const DSR::Node & node) { - // Create the message - auto node_msg = create_msg_node(node.name(), node.type()); - // Mark the node as deleted - node_msg.deleted = true; // Publish the message - node_to_ros_pub_->publish(node_msg); + node_to_ros_pub_->publish(to_msg(node, true)); RCLCPP_DEBUG( this->get_logger(), "The deleted node [%s] of type [%s] has been published through ROS", - node_msg.name.c_str(), node_msg.type.c_str()); + node.name().c_str(), node.type().c_str()); } void DSRBridge::edge_deleted(std::uint64_t from, std::uint64_t to, const std::string & edge_tag) @@ -353,18 +322,32 @@ void DSRBridge::edge_deleted(std::uint64_t from, std::uint64_t to, const std::st edge_msg.parent.c_str(), edge_msg.child.c_str(), edge_msg.type.c_str()); } -// Converter functions -std::optional DSRBridge::create_dsr_node(std::string name, std::string type) +DSR::Node DSRBridge::from_msg(const dsr_msgs::msg::Node & msg) { - if (!node_types::check_type(type)) { - throw std::runtime_error("Error, [" + type + "] is not a valid node type"); + if (!node_types::check_type(msg.type)) { + throw std::runtime_error("Error, [" + msg.type + "] is not a valid node type"); } + DSR::Node new_node; - new_node.name(name); - new_node.type(type); + new_node.name(msg.name); + new_node.type(msg.type); + modify_attributes(new_node, msg.attributes); return new_node; } +dsr_msgs::msg::Node DSRBridge::to_msg(const DSR::Node & node, bool deleted) +{ + dsr_msgs::msg::Node node_msg; + node_msg.header.stamp = this->now(); + node_msg.header.frame_id = source_; + node_msg.name = node.name(); + node_msg.type = node.type(); + node_msg.attributes = dsr_util::helpers::attributes_to_string(node.attrs()); + node_msg.deleted = deleted; + node_msg.updated = false; + return node_msg; +} + std::optional DSRBridge::create_dsr_edge( std::string from, std::string to, const std::string & type, std::vector & atts) { @@ -385,18 +368,6 @@ std::optional DSRBridge::create_dsr_edge( return {}; } -dsr_msgs::msg::Node DSRBridge::create_msg_node(std::string name, std::string type) -{ - dsr_msgs::msg::Node node_msg; - node_msg.header.stamp = this->now(); - node_msg.header.frame_id = source_; - node_msg.name = name; - node_msg.type = type; - node_msg.updated = false; - node_msg.deleted = false; - return node_msg; -} - dsr_msgs::msg::Edge DSRBridge::create_msg_edge( std::uint64_t from, std::uint64_t to, const std::string & type) { @@ -417,7 +388,7 @@ dsr_msgs::msg::Edge DSRBridge::create_msg_edge( // Helper functions template -void DSRBridge::modify_attributes(TYPE & elem, std::vector & att_str) +void DSRBridge::modify_attributes(TYPE & elem, const std::vector & att_str) { for (unsigned int i = 0; i < att_str.size(); i += 3) { std::string att_name = att_str[i]; @@ -425,7 +396,7 @@ void DSRBridge::modify_attributes(TYPE & elem, std::vector & att_st std::string att_type = att_str[i + 2]; // Add the attribute to the element - DSR::Attribute new_att = string_to_attribute(att_value, std::stoi(att_type)); + DSR::Attribute new_att = dsr_util::helpers::string_to_attribute(att_value, std::stoi(att_type)); elem.attrs().insert_or_assign(att_name, new_att); RCLCPP_DEBUG( this->get_logger(), @@ -434,68 +405,6 @@ void DSRBridge::modify_attributes(TYPE & elem, std::vector & att_st } } -std::string DSRBridge::attribute_to_string(const DSR::Attribute & att) -{ - std::locale::global(std::locale("C")); - switch (att.value().index()) { - case 0: { - return std::get(att.value()); - } - case 1: { - return std::to_string(std::get(att.value())); - } - case 2: { - return std::to_string(std::get(att.value())); - } - case 3: { - std::string att_str; - for (const auto & value : std::get>(att.value())) { - att_str += std::to_string(value) + std::string(","); - } - att_str.pop_back(); - return att_str; - } - case 4: { - return std::get(att.value()) ? "true" : "false"; - } - case 5: { - std::string att_str; - for (const auto & value : std::get>(att.value())) { - att_str += std::to_string(value) + std::string(","); - } - att_str.pop_back(); - return att_str; - } - case 6: { - return std::to_string(std::get(att.value())); - } - case 7: { - return std::to_string(std::get(att.value())); - } - case 8: { - return std::to_string(std::get(att.value())); - } - default: - return ""; - } -} - -std::vector DSRBridge::attributes_to_string( - const std::map & atts) -{ - std::vector att_vector_str; - for (const auto & [att_name, att_value] : atts) { - std::string att_str = attribute_to_string(att_value); - att_vector_str.push_back(att_name); - att_vector_str.push_back(att_str); - att_vector_str.push_back(get_type_from_attribute(att_value)); - RCLCPP_DEBUG( - this->get_logger(), - "Attribute [%s] = [%s]", att_name.c_str(), att_str.c_str()); - } - return att_vector_str; -} - template std::vector DSRBridge::attributes_updated_to_string( TYPE & elem, @@ -505,10 +414,10 @@ std::vector DSRBridge::attributes_updated_to_string( for (const auto & att_name : atts) { auto search = elem.attrs().find(att_name); if (search != elem.attrs().end()) { - std::string att_value = attribute_to_string(search->second); + std::string att_value = dsr_util::helpers::attribute_to_string(search->second); att_vector_str.push_back(att_name); att_vector_str.push_back(att_value); - att_vector_str.push_back(get_type_from_attribute(search->second)); + att_vector_str.push_back(dsr_util::helpers::get_type_from_attribute(search->second)); RCLCPP_DEBUG( this->get_logger(), "Attribute updated [%s] = [%s]", att_name.c_str(), att_value.c_str()); @@ -517,62 +426,6 @@ std::vector DSRBridge::attributes_updated_to_string( return att_vector_str; } -DSR::Attribute DSRBridge::string_to_attribute(const std::string & att_value, int att_type) -{ - DSR::Attribute new_att; - switch (att_type) { - case 0: { - new_att.value(std::string(att_value)); - break; - } - case 1: { - new_att.value(std::stoi(att_value)); - break; - } - case 2: { - if (att_value == "nan") { - new_att.value(std::numeric_limits::quiet_NaN()); - } else { - new_att.value(std::stof(att_value)); - } - break; - } - case 3: { - std::vector values = nav2_util::split(att_value, ','); - std::vector float_values; - for (const auto & value : values) { - if (value == "nan") { - float_values.push_back(std::numeric_limits::quiet_NaN()); - } else { - float_values.push_back(std::stof(value)); - } - } - new_att.value(float_values); - break; - } - case 4: { - new_att.value(att_value == "true"); - break; - } - case 6: { - new_att.value(std::stoull(att_value)); - break; - } - case 7: { - new_att.value(std::stoull(att_value)); - break; - } - default: - break; - } - return new_att; -} - -std::string DSRBridge::get_type_from_attribute(const DSR::Attribute & att) -{ - return std::to_string(att.value().index()); -} - } // namespace dsr_bridge #include "rclcpp_components/register_node_macro.hpp" diff --git a/dsr_bridge/test/CMakeLists.txt b/dsr_bridge/test/CMakeLists.txt index 696409a..aae7e7d 100644 --- a/dsr_bridge/test/CMakeLists.txt +++ b/dsr_bridge/test/CMakeLists.txt @@ -1,6 +1,4 @@ -find_package(dsr_util REQUIRED) - -# Test for test_bridge node +# Test for test_bridge ament_add_gtest(test_bridge test_bridge.cpp) target_link_libraries(test_bridge ${fastrtps_LIBRARIES} diff --git a/dsr_bridge/test/test_bridge.cpp b/dsr_bridge/test/test_bridge.cpp index b1a511b..655ecf9 100644 --- a/dsr_bridge/test/test_bridge.cpp +++ b/dsr_bridge/test/test_bridge.cpp @@ -32,9 +32,19 @@ class DSRBridgeFixture : public dsr_bridge::DSRBridge { return G_; } + + DSR::Node from_msg(const dsr_msgs::msg::Node & msg) + { + return dsr_bridge::DSRBridge::from_msg(msg); + } + + dsr_msgs::msg::Node to_msg(const DSR::Node & node, bool deleted) + { + return dsr_bridge::DSRBridge::to_msg(node, deleted); + } }; -TEST_F(DsrUtilTest, DSRBridgeFixtureConfigure) { +TEST_F(DsrUtilTest, DSRBridgeConfigure) { // Create the node auto agent_node = std::make_shared(); agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_)); @@ -48,6 +58,49 @@ TEST_F(DsrUtilTest, DSRBridgeFixtureConfigure) { agent_node->shutdown(); } +TEST_F(DsrUtilTest, DSRBridgeCreateDSRNode) { + // Create the node + auto agent_node = std::make_shared(); + agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_)); + + // Create the message + dsr_msgs::msg::Node node_msg; + node_msg.name = "robot_name"; + node_msg.type = "robot"; + node_msg.attributes = {"level", "5", "1"}; + + // Create the DSR node + auto dsr_node = agent_node->from_msg(node_msg); + EXPECT_EQ(dsr_node.name(), node_msg.name); + EXPECT_EQ(dsr_node.type(), node_msg.type); + auto attributes = dsr_node.attrs(); + auto search = attributes.find("level"); + EXPECT_TRUE(search != attributes.end()); + EXPECT_EQ(std::get(search->second.value()), 5); +} + +TEST_F(DsrUtilTest, DSRBridgeCreateMsgNode) { + // Create the node + auto agent_node = std::make_shared(); + agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_)); + agent_node->configure(); + agent_node->activate(); + + // Create the DSR node + auto dsr_node = DSR::Node::create("robot_name"); + agent_node->get_graph()->add_or_modify_attrib_local(dsr_node, 5); + + // Create the message + auto node_msg = agent_node->to_msg(dsr_node, true); + EXPECT_EQ(node_msg.name, dsr_node.name()); + EXPECT_EQ(node_msg.type, dsr_node.type()); + auto attributes = dsr_node.attrs(); + auto search = attributes.find("level"); + EXPECT_TRUE(search != attributes.end()); + EXPECT_EQ(std::get(search->second.value()), 5); + EXPECT_TRUE(node_msg.deleted); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/dsr_util/CHANGELOG.rst b/dsr_util/CHANGELOG.rst index af8282c..534e870 100644 --- a/dsr_util/CHANGELOG.rst +++ b/dsr_util/CHANGELOG.rst @@ -13,6 +13,7 @@ X.Y.Z (DD-MM-YYYY) * Added unit tests. * Added bond. * Added service agent. +* Added helpers functions from dsr_bridge. 0.1.0 (04-03-2024) ------------------ diff --git a/dsr_util/include/dsr_util/helpers.hpp b/dsr_util/include/dsr_util/helpers.hpp new file mode 100644 index 0000000..b91cd30 --- /dev/null +++ b/dsr_util/include/dsr_util/helpers.hpp @@ -0,0 +1,194 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DSR_UTIL__HELPERS_HPP_ +#define DSR_UTIL__HELPERS_HPP_ + +#include +#include +#include +#include +#include + +#include + +// ROS +#include "dsr/core/types/common_types.h" +#include "dsr/core/types/type_checking/dsr_edge_type.h" +#include "dsr/core/types/type_checking/dsr_node_type.h" + +namespace dsr_util::helpers +{ + +/** + * @brief Convert a string to a DSR::Attribute. + * + * @param att_value The string value of the attribute. + * @param att_type The type of the attribute. + * @return DSR::Attribute The DSR attribute created. + */ +DSR::Attribute string_to_attribute(const std::string & att_value, int att_type) +{ + DSR::Attribute new_att; + switch (att_type) { + case 0: { + new_att.value(std::string(att_value)); + break; + } + case 1: { + new_att.value(std::stoi(att_value)); + break; + } + case 2: { + if (att_value == "nan") { + new_att.value(std::numeric_limits::quiet_NaN()); + } else { + new_att.value(std::stof(att_value)); + } + break; + } + case 3: { + std::vector values; + boost::split(values, att_value, boost::is_any_of(",")); + std::vector float_values; + for (const auto & value : values) { + if (value == "nan") { + float_values.push_back(std::numeric_limits::quiet_NaN()); + } else { + float_values.push_back(std::stof(value)); + } + } + new_att.value(float_values); + break; + } + case 4: { + new_att.value(att_value == "true"); + break; + } + case 5: { + std::vector values; + boost::split(values, att_value, boost::is_any_of(",")); + std::vector uint_values; + for (const auto & value : values) { + uint_values.push_back(std::stoi(value)); + } + new_att.value(uint_values); + break; + } + case 6: { + new_att.value(static_cast(std::stoi(att_value))); + break; + } + case 7: { + new_att.value(std::stoull(att_value)); + break; + } + case 8: { + if (att_value == "nan") { + new_att.value(std::numeric_limits::quiet_NaN()); + } else { + new_att.value(std::stod(att_value)); + } + break; + } + default: + break; + } + return new_att; +} + +/** + * @brief Convert a DSR::Attribute to a string. + * + * @param att The DSR attribute. + * @return std::string The string representation of the attribute. + */ +std::string attribute_to_string(const DSR::Attribute & att) +{ + std::locale::global(std::locale("C")); + switch (att.value().index()) { + case 0: { + return std::get(att.value()); + } + case 1: { + return std::to_string(std::get(att.value())); + } + case 2: { + return std::to_string(std::get(att.value())); + } + case 3: { + std::string att_str; + for (const auto & value : std::get>(att.value())) { + att_str += std::to_string(value) + std::string(","); + } + att_str.pop_back(); + return att_str; + } + case 4: { + return std::get(att.value()) ? "true" : "false"; + } + case 5: { + std::string att_str; + for (const auto & value : std::get>(att.value())) { + att_str += std::to_string(value) + std::string(","); + } + att_str.pop_back(); + return att_str; + } + case 6: { + return std::to_string(std::get(att.value())); + } + case 7: { + return std::to_string(std::get(att.value())); + } + case 8: { + return std::to_string(std::get(att.value())); + } + default: + return ""; + } +} + +/** + * @brief Get the type from attribute object + * + * @param att The DSR attribute. + * @return std::string The string representation of the attribute. + */ +std::string get_type_from_attribute(const DSR::Attribute & att) +{ + return std::to_string(att.value().index()); +} + +/** + * @brief Convert a map of attributes to a vector of strings. + * + * @param atts The map of attributes. + * @return std::vector The vector of strings. + */ +std::vector attributes_to_string(const std::map & atts) +{ + std::vector att_vector_str; + for (const auto & [att_name, att_value] : atts) { + att_vector_str.push_back(att_name); + att_vector_str.push_back(dsr_util::helpers::attribute_to_string(att_value)); + att_vector_str.push_back(dsr_util::helpers::get_type_from_attribute(att_value)); + } + return att_vector_str; +} + +} // namespace dsr_util::helpers + +#endif // DSR_UTIL__HELPERS_HPP_ diff --git a/dsr_util/test/CMakeLists.txt b/dsr_util/test/CMakeLists.txt index c7defad..399ef44 100644 --- a/dsr_util/test/CMakeLists.txt +++ b/dsr_util/test/CMakeLists.txt @@ -18,6 +18,19 @@ target_include_directories(test_dsr_api_ext PUBLIC target_link_libraries(test_dsr_api_ext agent_node ${fastrtps_LIBRARIES} + ${dsr_LIBRARIES} + ament_index_cpp::ament_index_cpp +) + +# Test for test_helpers +ament_add_gtest(test_helpers test_helpers.cpp) +target_include_directories(test_helpers PUBLIC + "$" + "$" +) +target_link_libraries(test_helpers + ${fastrtps_LIBRARIES} + ${dsr_LIBRARIES} ament_index_cpp::ament_index_cpp ) diff --git a/dsr_util/test/test_helpers.cpp b/dsr_util/test/test_helpers.cpp new file mode 100644 index 0000000..e773832 --- /dev/null +++ b/dsr_util/test/test_helpers.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2024 Alberto J. Tudela Roldán +// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "dsr_util/helpers.hpp" + +TEST(DsrUtilTest, StringToAttribute) { + // Test string + auto att = dsr_util::helpers::string_to_attribute("test", 0); + EXPECT_EQ(std::get(att.value()), "test"); + + // Test int + att = dsr_util::helpers::string_to_attribute("5", 1); + EXPECT_EQ(std::get(att.value()), 5); + + // Test float + att = dsr_util::helpers::string_to_attribute("5.0", 2); + EXPECT_FLOAT_EQ(std::get(att.value()), 5.0); + + // Test float nan + att = dsr_util::helpers::string_to_attribute("nan", 2); + EXPECT_TRUE(std::isnan(std::get(att.value()))); + + // Test float vector + att = dsr_util::helpers::string_to_attribute("5.0,nan,7.0", 3); + auto values = std::get>(att.value()); + EXPECT_FLOAT_EQ(values[0], 5.0); + EXPECT_TRUE(std::isnan(values[1])); + EXPECT_FLOAT_EQ(values[2], 7.0); + + // Test bool + att = dsr_util::helpers::string_to_attribute("true", 4); + EXPECT_TRUE(std::get(att.value())); + + // Test uint8_t vector + att = dsr_util::helpers::string_to_attribute("5,6,7", 5); + auto uint_values = std::get>(att.value()); + EXPECT_EQ(uint_values[0], 5); + EXPECT_EQ(uint_values[1], 6); + EXPECT_EQ(uint_values[2], 7); + + // Test uint32_t + att = dsr_util::helpers::string_to_attribute("5", 6); + EXPECT_EQ(std::get(att.value()), 5); + + // Test uint64_t + att = dsr_util::helpers::string_to_attribute("5", 7); + EXPECT_EQ(std::get(att.value()), 5); + + // Test double + att = dsr_util::helpers::string_to_attribute("5.0", 8); + EXPECT_DOUBLE_EQ(std::get(att.value()), 5.0); + + // Test double nan + att = dsr_util::helpers::string_to_attribute("nan", 8); + EXPECT_TRUE(std::isnan(std::get(att.value()))); + + // Test unknown type + att = dsr_util::helpers::string_to_attribute("test", 15); +} + +TEST(DsrUtilTest, AttributeToString) { + // Test string + DSR::Attribute att; + att.value("test"); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "test"); + + // Test int + att.value(5); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "5"); + + // Test float + att.value(5.0); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "5.000000"); + + // Test float vector + att.value(std::vector{5.0, std::numeric_limits::quiet_NaN(), 7.0}); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "5.000000,nan,7.000000"); + + // Test bool + att.value(true); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "true"); + + // Test uint8_t vector + att.value(std::vector{5, 6, 7}); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "5,6,7"); + + // Test uint32_t + att.value(5); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "5"); + + // Test uint64_t + att.value(5); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "5"); + + // Test double + att.value(5.0); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), "5.000000"); + + // Test unknown type + att.value(std::vector{5, 6, 7}); + EXPECT_EQ(dsr_util::helpers::attribute_to_string(att), ""); +} + +TEST(DsrUtilTest, AttributesToString) { + std::map atts; + DSR::Attribute att; + att.value(1.0); + atts["pos_x"] = att; + att.value(2.0); + atts["pos_y"] = att; + + auto att_str = dsr_util::helpers::attributes_to_string(atts); + EXPECT_EQ(att_str.size(), 6); + EXPECT_EQ(att_str[0], "pos_x"); + EXPECT_EQ(att_str[1], "1.000000"); + EXPECT_EQ(att_str[2], "8"); + EXPECT_EQ(att_str[3], "pos_y"); + EXPECT_EQ(att_str[4], "2.000000"); + EXPECT_EQ(att_str[5], "8"); +} + +TEST(DsrUtilTest, GetAttributeType) { + DSR::Attribute att; + att.value("test"); + EXPECT_EQ(dsr_util::helpers::get_type_from_attribute(att), "0"); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + bool success = RUN_ALL_TESTS(); + return success; +}