alb_publisher_test.cpp

Go to the documentation of this file.

// GTest headers
#include <gtest/gtest.h>

// ROS headers
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/TimeReference.h"
#include "tf2_msgs/TFMessage.h"

// Osef headers
#include "tlvParser.h"

// Local headers
#include "alb_common.h"
#include "alb_publisher.h"
#include "outsight_alb_driver/AugmentedCloud.h"
#include "outsight_alb_driver/TrackedObjects.h"

// Constant definition of test data.
constexpr const char *k_alb_tracking_file = "tracking_test_file";
constexpr const char *k_alb_slam_file = "slam_test_file";

constexpr const size_t k_tracking_messages = 60;
constexpr const size_t k_slam_messages = 54;

class AlbPublisherTest : public ::testing::Test {
    protected:
        void SetUp() override
        {
        }

        void TearDown() override
        {
                // Delete all test parameters.
                removeParameters();

                // Free memory.
                if (file) {
                        fclose(file);
                }
                delete (buffer);
        }

        void openFile(const std::string &key)
        {
                std::string filename("");
                ros::NodeHandle private_node("~");

                if (private_node.hasParam(key)) {
                        private_node.param<std::string>(key, filename, "");
                }

                file = fopen(filename.c_str(), "r");
        }

        void setTestParam(const std::string &key, bool value)
        {
                ros::NodeHandle private_node("~");
                private_node.setParam(key, value);

                custom_parameters.push_back(key);
        }

        void removeParameters()
        {
                ros::NodeHandle private_node("~");

                for (auto key : custom_parameters) {
                        private_node.deleteParam(key);
                }
        }

        template <typename ROSMessageType>
        void subscribeTo(const std::string &topic)
        {
                alb_subscriber = node.subscribe<ROSMessageType>(
                        topic, 1000, &AlbPublisherTest::onMessageReceived<ROSMessageType>, this);
        }

        template <typename ROSMessageType>
        void onMessageReceived(const typename ROSMessageType::ConstPtr &msg)
        {
                received_messages++;
        }

        void readAndPublish(AlbPublisher &albPublisher)
        {
                // Read and publish TLV data.
                Tlv::tlv_s *read_tlv = nullptr;
                ros::Rate loop_rate(100);

                while (read_tlv = Tlv::readFromFile(file, buffer, buffer_size)) {
                        albPublisher.publish(read_tlv);
                        ros::spinOnce();
                        loop_rate.sleep();
                }
        }

    protected:
        FILE *file = nullptr;
        uint8_t *buffer = nullptr;
        size_t buffer_size = 0;
        size_t received_messages = 0;

        std::vector<std::string> custom_parameters;

        ros::NodeHandle node;
        ros::Subscriber alb_subscriber;
};

TEST_F(AlbPublisherTest, openTrackingFile)
{
        openFile(k_alb_tracking_file);
        ASSERT_FALSE(!file);
}

TEST_F(AlbPublisherTest, openSlamFile)
{
        openFile(k_alb_slam_file);
        ASSERT_FALSE(!file);
}

TEST_F(AlbPublisherTest, publishPointCloud)
{
        // Open slam file.
        openFile(k_alb_slam_file);
        ASSERT_FALSE(!file);

        // Set parameter to publish PointCloud message.
        setTestParam(ALBCommon::k_alb_point_cloud_config, true);
        subscribeTo<sensor_msgs::PointCloud2>(ALBCommon::k_alb_point_cloud);

        AlbPublisher alb_publisher(node);
        EXPECT_EQ(alb_subscriber.getNumPublishers(), 1);

        readAndPublish(alb_publisher);
        EXPECT_EQ(received_messages, k_slam_messages);
}

TEST_F(AlbPublisherTest, publishPose)
{
        // Open tracking file.
        openFile(k_alb_slam_file);
        ASSERT_FALSE(!file);

        // Set parameter to publish Pose message.
        setTestParam(ALBCommon::k_alb_pose_config, true);
        subscribeTo<geometry_msgs::PoseStamped>(ALBCommon::k_alb_pose);

        AlbPublisher alb_publisher(node);
        EXPECT_EQ(alb_subscriber.getNumPublishers(), 1);

        readAndPublish(alb_publisher);
        EXPECT_EQ(received_messages, k_slam_messages);
}

TEST_F(AlbPublisherTest, publishTimeReference)
{
        // Open tracking file.
        openFile(k_alb_tracking_file);
        ASSERT_FALSE(!file);

        // Set parameter to publish TimeReference message.
        setTestParam(ALBCommon::k_alb_time_ref_config, true);
        subscribeTo<sensor_msgs::TimeReference>(ALBCommon::k_alb_time_ref);

        AlbPublisher alb_publisher(node);
        EXPECT_EQ(alb_subscriber.getNumPublishers(), 1);

        readAndPublish(alb_publisher);
        EXPECT_EQ(received_messages, k_tracking_messages);
}

TEST_F(AlbPublisherTest, publishTrackedObjects)
{
        // Open tracking file.
        openFile(k_alb_tracking_file);
        ASSERT_FALSE(!file);

        // Set parameter to publish TrackedObjects message.
        setTestParam(ALBCommon::k_alb_objects_config, true);
        subscribeTo<outsight_alb_driver::TrackedObjects>(ALBCommon::k_alb_objects);

        AlbPublisher alb_publisher(node);
        EXPECT_EQ(alb_subscriber.getNumPublishers(), 1);

        readAndPublish(alb_publisher);
        EXPECT_EQ(received_messages, k_tracking_messages);
}

TEST_F(AlbPublisherTest, publishEgomotion)
{
        // Open tracking file.
        openFile(k_alb_tracking_file);
        ASSERT_FALSE(!file);

        // Set parameter to publish Egomotion message.
        setTestParam(ALBCommon::k_alb_egomotion_config, true);
        subscribeTo<geometry_msgs::PoseStamped>(ALBCommon::k_alb_egomotion);

        AlbPublisher alb_publisher(node);
        EXPECT_EQ(alb_subscriber.getNumPublishers(), 1);

        readAndPublish(alb_publisher);
        EXPECT_EQ(received_messages, k_tracking_messages);
}

TEST_F(AlbPublisherTest, publishOdometry)
{
        // Open tracking file.
        openFile(k_alb_tracking_file);
        ASSERT_FALSE(!file);

        // Set parameter to publish Odometry message.
        setTestParam(ALBCommon::k_alb_odometry_config, true);
        subscribeTo<nav_msgs::Odometry>(ALBCommon::k_alb_odometry);

        AlbPublisher alb_publisher(node);
        EXPECT_EQ(alb_subscriber.getNumPublishers(), 1);

        readAndPublish(alb_publisher);
        EXPECT_EQ(received_messages, k_tracking_messages);
}

TEST_F(AlbPublisherTest, publishAugmentedCloud)
{
        // Open slam file.
        openFile(k_alb_slam_file);
        ASSERT_FALSE(!file);

        // Set parameter to publish PointCloud message.
        setTestParam(ALBCommon::k_alb_augmented_cloud_config, true);
        subscribeTo<outsight_alb_driver::AugmentedCloud>(ALBCommon::k_alb_augmented_cloud);

        AlbPublisher alb_publisher(node);
        EXPECT_EQ(alb_subscriber.getNumPublishers(), 1);

        readAndPublish(alb_publisher);
        EXPECT_EQ(received_messages, k_slam_messages);
}

TEST_F(AlbPublisherTest, broadcastAlbTransform)
{
        // Open slam file.
        openFile(k_alb_slam_file);
        ASSERT_FALSE(!file);

        // Set parameter to publish Alb fixed frame to sensor frame tf.
        setTestParam(ALBCommon::k_alb_broadcast_tf_config, true);
        setTestParam(ALBCommon::k_alb_odometry_config, true);
        subscribeTo<tf2_msgs::TFMessage>("tf");

        AlbPublisher alb_publisher(node);

        readAndPublish(alb_publisher);
        EXPECT_EQ(received_messages, k_slam_messages);
}

int main(int argc, char **argv)
{
        testing::InitGoogleTest(&argc, argv);
        ros::init(argc, argv, "outsight_publishers_test");
        return RUN_ALL_TESTS();
}