Page History

...

#include <chrono>
#include <iostream>
#include <thread>

#include <dds/pub/ddspub.hpp>

#include "cpm/init.hpp"
#include "cpm/Logging.hpp"
#include "cpm/ParticipantSingleton.hpp"
#include "cpm/get_topic.hpp"
#include "cpm/get_time_ns.hpp"
#include "Color.hpp"
#include "Visualization.hpp"

int main(int argc, char *argv[]) {
    //Initialize the cpm library
    cpm::init(argc, argv);
    cpm::Logging::Instance().set_id("Visualization_test");

    std::cout << "Creating visualization sender..." << std::endl;

    //Create a RTI DataWriter to send visualization messages to the LCC in a reliable way, using the cpm's 'standard participant' for communication which is already properly initialized
    dds::pub::DataWriter<Visualization> viz_writer(dds::pub::Publisher(cpm::ParticipantSingleton::Instance()), cpm::get_topic<Visualization>("visualization"), dds::pub::qos::DataWriterQos() << dds::core::policy::Reliability::Reliable());

    //Creation of a line visualization message ----------------------------
    Visualization viz;
    viz.id("1");
    viz.type(VisualizationType::LineStrips);
    viz.time_to_live(3000000000);
    viz.size(1.0);

    Point2D point1(0.0, 0.0);
    Point2D point2(1.0, 1.0);
    Point2D point3(2.0, -2.0);
    Point2D point4(3.0, 3.0);
    Point2D point5(4.0, -4.0);
    std::vector<Point2D> viz_points {point1, point2, point3, point4, point5};
    viz.points(rti::core::vector<Point2D>(viz_points)); //In newer RTI versions, a conversion of the vector data structure to RTI's own implementation might become obsolete, but it is required here

    Color viz_color(255, 0, 255, 255);
    viz.color(viz_color);
    //Message created -----------------------------------------------------

    //Always wait a bit between sending messages, or else messages might get lost
    usleep(100000);

    std::cout << "Sending visualization line..." << std::endl;

    //Send the line visualization message to the LCC
    viz_writer.write(viz);

    //Creation of a polygon message ---------------------------------------
    Visualization viz2;
    viz2.id("2");
    viz2.type(VisualizationType::Polygon);
    viz2.time_to_live(5000000000);
    viz2.size(0.05);

    Point2D point1_2(0.0, 0.0);
    Point2D point2_2(0.5, 0.5);
    Point2D point3_2(1.0, 0.0);
    std::vector<Point2D> viz2_points {point1_2, point2_2, point3_2};
    viz2.points(rti::core::vector<Point2D>(viz2_points));

    Color viz2_color(255, 255, 0, 255);
    viz2.color(viz2_color);
    //Message created -----------------------------------------------------

    usleep(100000);

    std::cout << "Sending visualization polygon..." << std::endl;

    //Send the polygon message
    viz_writer.write(viz2);

    //Creation of a string message ----------------------------------------
    Visualization viz3;
    viz3.id("3");
    viz3.type(VisualizationType::StringMessage);
    viz3.time_to_live(10000000000);
    viz3.size(1.0);

    Point2D point1_3(0.2, 0.2);
    std::vector<Point2D> viz3_points {point1_3};
    viz3.points(rti::core::vector<Point2D>(viz3_points));

    Color viz3_color(255, 255, 255, 0);
    viz3.color(viz3_color);

    viz3.string_message("Hello LCC!");
    //Message created -----------------------------------------------------

    usleep(100000);

    std::cout << "Sending visualization string..." << std::endl;

    //Send the string message
    viz_writer.write(viz3);

    std::cout << "Shutting down..." << std::endl;

    return 0;
}