mvsim-subscriber-example.cpp

Go to the documentation of this file.

#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mvsim/Comms/Client.h>
#include <mvsim/mvsim-msgs/GenericObservation.pb.h>
#include <mvsim/mvsim-msgs/TimeStampedPose.pb.h>

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

void myPoseCallback(const mvsim_msgs::TimeStampedPose& p)
{
        std::cout << "topic callback: " << p.DebugString() << std::endl;
}

void mySensorCallback(const mvsim_msgs::GenericObservation& o)
{
        const std::vector<uint8_t> data(
                o.mrptserializedobservation().begin(),
                o.mrptserializedobservation().end());

        mrpt::serialization::CSerializable::Ptr obj;
        mrpt::serialization::OctetVectorToObject(data, obj);

        mrpt::obs::CObservation::Ptr obs =
                std::dynamic_pointer_cast<mrpt::obs::CObservation>(obj);
        ASSERT_(obs);

        std::cout << "sensor callback: " << obs->asString() << "\n";
}

int main(int argc, char** argv)
{
        try
        {
                mvsim::Client client("mvsim-subscriber-example");
                client.setVerbosityLevel(mrpt::system::LVL_DEBUG);

                client.connect();

                client.subscribeTopic<mvsim_msgs::TimeStampedPose>(
                        "/r1/pose", myPoseCallback);

                client.subscribeTopic<mvsim_msgs::GenericObservation>(
                        "/r1/laser1", mySensorCallback);

                std::this_thread::sleep_for(std::chrono::seconds(5));

                return 0;
        }
        catch (const std::exception& e)
        {
                std::cerr << mrpt::exception_to_str(e);
                return 1;
        }
}