asrVisualizer.h

Go to the documentation of this file.

#pragma once

#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include "gnuplot-iostream.h"

#include <string>
#include <set>
#include <utility>

#include <Eigen/Core>
#include "AttributedPoint.h"
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

namespace ASR
{

    class asrVisualizer
    {
    public:
        asrVisualizer(int max_number_of_published_vis_msgs = 100);

        void initAnimatedPlot(const std::string& pPlotTitle,
                              const std::string& pXLabel,
                              const std::string& pYLabel,
                              const std::pair<float, float>& pXRange,
                              const std::pair<float, float>& pYRange,
                              const std::pair<float, float>& pDelta);



        void sendPlotToGnuplot();

        void addPointToFoundBuffer(float x, float y) { addPointToFoundBuffer(Eigen::Vector2f(x,y)); }
        void addPointToFoundBuffer(Eigen::Vector2f point);

        void publishRVizMarker(std::vector<AttributedPoint> hypotheses, std::vector<AttributedPoint> found_poses,
                           const std::string target_frame = "/map", const int id = 1);


        void addPointToUnfoundBuffer(Eigen::Vector2f point);

        void addAttributedPoints(std::vector<AttributedPoint> hypotheses, std::vector<AttributedPoint> found_poses);


        void generateMarkerArray(std::vector<AttributedPoint> hypotheses,
                                 std::vector<AttributedPoint> found_poses);

        void publishMarkerArray();


    private:
        ros::Publisher publisher;
        visualization_msgs::MarkerArray markerArray;

        double marker_lifetime;

        //Interface with which configurations or data is sent to gnuplot.
        boost::shared_ptr<Gnuplot> mGnuplotHandler;

        // the publisher to publish marker msgs to rviz
        ros::Publisher hypothesis3dPublisher;

        //buffers for visualization points
        std::vector<std::pair<double, double> > mUnfoundBuffer;
        std::vector<std::pair<double, double> > mFoundBuffer;
        std::vector<std::pair<float, float> > mVisitedViews;

    };


    typedef boost::shared_ptr<asrVisualizer> asrVisualizerPtr;

}