38#include <rclcpp/rclcpp.hpp>
39#include <gtest/gtest.h>
41TEST(RDFIntegration, default_arguments)
43 rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>(
"default_arguments");
45 ASSERT_NE(
nullptr, loader.
getURDF());
46 EXPECT_EQ(
"kermit", loader.
getURDF()->name_);
47 ASSERT_NE(
nullptr, loader.
getSRDF());
48 EXPECT_EQ(
"kermit", loader.
getSRDF()->getName());
51TEST(RDFIntegration, non_existent)
53 rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>(
"non_existent");
55 ASSERT_EQ(
nullptr, loader.
getURDF());
56 ASSERT_EQ(
nullptr, loader.
getSRDF());
59TEST(RDFIntegration, topic_based)
61 rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>(
"topic_based");
63 ASSERT_NE(
nullptr, loader.
getURDF());
64 EXPECT_EQ(
"gonzo", loader.
getURDF()->name_);
65 ASSERT_NE(
nullptr, loader.
getSRDF());
66 EXPECT_EQ(
"gonzo", loader.
getSRDF()->getName());
69TEST(RDFIntegration, executor)
74 rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>(
"executor");
77 rclcpp::executors::SingleThreadedExecutor executor;
78 executor.add_node(node);
79 std::thread spinning_thread([&executor] { executor.spin(); });
85 ASSERT_NE(
nullptr, loader.getURDF());
86 EXPECT_EQ(
"gonzo", loader.getURDF()->name_);
87 ASSERT_NE(
nullptr, loader.getSRDF());
88 EXPECT_EQ(
"gonzo", loader.getSRDF()->getName());
90 spinning_thread.join();
93TEST(RDFIntegration, xacro_test)
96 std::vector<std::string> xacro_args;
98 "rdf_loader/test/data/robin.srdf.xacro", xacro_args));
99 EXPECT_GT(output.size(), 0u);
104 rclcpp::init(argc, argv);
105 ::testing::InitGoogleTest(&argc, argv);
107 int ret = RUN_ALL_TESTS();
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
static bool loadPkgFileToString(std::string &buffer, const std::string &package_name, const std::string &relative_path, const std::vector< std::string > &xacro_args)
helper that generates a file path based on package name and relative file path to package
const srdf::ModelSharedPtr & getSRDF() const
Get the parsed SRDF model.
int main(int argc, char **argv)
TEST(RDFIntegration, default_arguments)