109 lines
3.5 KiB
C++
109 lines
3.5 KiB
C++
#include <algorithm>
|
|
#include <chrono>
|
|
#include <string>
|
|
|
|
#include <gtest/gtest.h>
|
|
#include <rclcpp/rclcpp.hpp>
|
|
#include <sensor_msgs/msg/compressed_image.hpp>
|
|
#include <sensor_msgs/msg/image.hpp>
|
|
#include <origincar_msg/srv/speak.hpp>
|
|
|
|
#define private public
|
|
#define main qr_detect_node_main
|
|
#include "../src/qr_detect.cpp"
|
|
#undef main
|
|
#undef private
|
|
|
|
namespace
|
|
{
|
|
|
|
bool has_subscription_type(
|
|
const std::vector<rclcpp::TopicEndpointInfo> & endpoints,
|
|
const std::string & topic_type)
|
|
{
|
|
return std::any_of(
|
|
endpoints.begin(), endpoints.end(),
|
|
[&](const rclcpp::TopicEndpointInfo & endpoint) {
|
|
return endpoint.topic_type() == topic_type;
|
|
});
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TEST(QrDetectNode, subscribesToCompressedImageWhenCompressedPublisherExists)
|
|
{
|
|
if (!rclcpp::ok()) {
|
|
rclcpp::init(0, nullptr);
|
|
}
|
|
|
|
auto camera_node = std::make_shared<rclcpp::Node>("qr_detect_compressed_camera_test");
|
|
auto camera_pub = camera_node->create_publisher<sensor_msgs::msg::CompressedImage>(
|
|
"/qr_test_image", rclcpp::SensorDataQoS());
|
|
|
|
rclcpp::NodeOptions options;
|
|
options.append_parameter_override("image_topic", "/qr_test_image");
|
|
auto qr_node = std::make_shared<QrDetectNode>(options);
|
|
auto graph_node = std::make_shared<rclcpp::Node>("qr_detect_graph_test");
|
|
rclcpp::executors::SingleThreadedExecutor executor;
|
|
executor.add_node(camera_node);
|
|
executor.add_node(qr_node);
|
|
executor.add_node(graph_node);
|
|
|
|
std::vector<rclcpp::TopicEndpointInfo> endpoints;
|
|
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
|
|
while (std::chrono::steady_clock::now() < deadline) {
|
|
sensor_msgs::msg::CompressedImage msg;
|
|
msg.format = "jpeg";
|
|
msg.data = {0xff, 0xd8, 0xff, 0xd9};
|
|
camera_pub->publish(msg);
|
|
executor.spin_some();
|
|
endpoints = graph_node->get_subscriptions_info_by_topic("/qr_test_image");
|
|
if (has_subscription_type(endpoints, "sensor_msgs/msg/CompressedImage")) {
|
|
break;
|
|
}
|
|
rclcpp::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
|
|
EXPECT_TRUE(has_subscription_type(endpoints, "sensor_msgs/msg/CompressedImage"));
|
|
}
|
|
|
|
TEST(QrDetectNode, publishesQrResultsOnly)
|
|
{
|
|
if (!rclcpp::ok()) {
|
|
rclcpp::init(0, nullptr);
|
|
}
|
|
|
|
auto qr_node = std::make_shared<QrDetectNode>();
|
|
auto graph_node = std::make_shared<rclcpp::Node>("qr_detect_publication_graph_test");
|
|
rclcpp::executors::SingleThreadedExecutor executor;
|
|
executor.add_node(qr_node);
|
|
executor.add_node(graph_node);
|
|
|
|
std::vector<rclcpp::TopicEndpointInfo> qr_results_publishers;
|
|
std::vector<rclcpp::TopicEndpointInfo> vlm_result_publishers;
|
|
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
|
|
while (std::chrono::steady_clock::now() < deadline) {
|
|
executor.spin_some();
|
|
qr_results_publishers = graph_node->get_publishers_info_by_topic("/qr_results");
|
|
vlm_result_publishers = graph_node->get_publishers_info_by_topic("/vlm_result");
|
|
if (has_subscription_type(qr_results_publishers, "std_msgs/msg/String")) {
|
|
break;
|
|
}
|
|
rclcpp::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
|
|
EXPECT_TRUE(has_subscription_type(qr_results_publishers, "std_msgs/msg/String"));
|
|
EXPECT_FALSE(has_subscription_type(vlm_result_publishers, "std_msgs/msg/String"));
|
|
}
|
|
|
|
TEST(QrDetectNode, createsTtsSpeakClient)
|
|
{
|
|
if (!rclcpp::ok()) {
|
|
rclcpp::init(0, nullptr);
|
|
}
|
|
|
|
auto qr_node = std::make_shared<QrDetectNode>();
|
|
ASSERT_NE(qr_node->tts_client_, nullptr);
|
|
EXPECT_STREQ(qr_node->tts_client_->get_service_name(), "/tts/speak");
|
|
}
|