model_visualizer_rviz.hpp

Go to the documentation of this file.

#pragma once
//Local includes
#include <asr_ism_visualizations/visualizer_rviz.hpp>
#include <asr_ism_visualizations/VizHelperRVIZ.hpp>

//ISM inlcudes
#include <ISM/common_type/VoteSpecifier.hpp>
#include <ISM/common_type/Tracks.hpp>


namespace VIZ
{
class ModelVisualizerRVIZ : public VisualizerRVIZ{

public:
    ModelVisualizerRVIZ(ros::Publisher publisher, std::string base_frame, std::string prefix, double marker_lifetime)
        : VisualizerRVIZ(publisher), base_frame_(base_frame), prefix_(prefix), marker_lifetime_(marker_lifetime)
    {};


    void addVisualization(const std::string pattern_name, const ISM::TracksPtr tracks, const ISM::TracksPtr ism_object_tracks,
                          const std::map<int, std::vector<ISM::VoteSpecifierPtr>> track_index_to_votes);


private:

    visualization_msgs::MarkerArray generateModelMarkers(std::string pattern_name, const ISM::TracksPtr tracks, const std::map<int, std::vector<ISM::VoteSpecifierPtr>> track_index_to_votes);

    void initObjectToColorMap(ISM::TracksPtr tracks, ISM::TracksPtr ism_object_tracks);

    std::string base_frame_;
    std::string prefix_;
    double marker_lifetime_;
    std::map<std::string, std::map<std::string, std_msgs::ColorRGBA>> object_to_color_map_;
};
}