ism_voting_visualizer_rviz.hpp

Go to the documentation of this file.

#pragma once
//Ilcas includes
#include <asr_ism_visualizations/visualizer_rviz.hpp>
#include <asr_ism_visualizations/VizHelperRVIZ.hpp>
#include <ISM/common_type/RecognitionResult.hpp>
#include <ISM/typedef.hpp>

//ROS includes
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <asr_ism_visualizations/ism_voting_visualizerConfig.h>

namespace VIZ
{
class ISMVotingVisualizerRVIZ : public VisualizerRVIZ
{
public:
    ISMVotingVisualizerRVIZ(const ros::Publisher& publisher, double bin_size, const ros::NodeHandle& nh)
        : VisualizerRVIZ(publisher), bin_size_(bin_size)
    {
        reconfigure_server_ = new dynamic_reconfigure::Server<asr_ism_visualizations::ism_voting_visualizerConfig>(nh);
        dynamic_reconfigure::Server<asr_ism_visualizations::ism_voting_visualizerConfig>::CallbackType reconf_callback = boost::bind(&ISMVotingVisualizerRVIZ::dynamicReconfCallback, this, _1, _2);
        reconfigure_server_->setCallback(reconf_callback);
    }

    ~ISMVotingVisualizerRVIZ() {
    }

    void releaseCallback() {
        reconfigure_server_->clearCallback();
        delete reconfigure_server_;

    }

    void dynamicReconfCallback(asr_ism_visualizations::ism_voting_visualizerConfig &config, uint32_t level) {
        ROS_DEBUG_STREAM("Voting vizualizer dyn-params updated.");

        line_scale_ = config.lineScale;
        marker_lifetime_ = config.markerLifetime;
        base_frame_ = config.baseFrame;
        axis_scale_ = config.axisScale;
        grid_scale_ = config.gridScale;
        object_marker_scale_ = config.objectMarkerScale;
        bin_opacity_ = config.binOpacity;
        grid_opacity_ = config.gridOpacity;
        vote_opacity_ = config.voteOpacity;
        object_marker_opacity_ = config.objectMarkerOpacity;
        confidence_sphere_opacity_ = config.confidenceSphereOpacity;
        pattern_name_ = config.patternname;
        scene_name_ = config.scenename;
        bin_scale_ = config.binScale;
    }


    void addVisualization(const ISM::PatternNameAndVotingSpaceTuple& voting_space, std::vector<ISM::RecognitionResultPtr> results);

    std::string getPatternName(){
        return pattern_name_;
    }

private:

    visualization_msgs::Marker generateGridMarker(std::tuple<int, int, int, int, int, int> grid_bb);

    visualization_msgs::MarkerArray generateBinAndGridMarker(ISM::VotingSpacePtr voting_space_ptr);

    visualization_msgs::MarkerArray generateVoteMarker(std::map<ISM::ObjectPtr, std::vector<ISM::PosePtr>> voter_to_poses_map);

    visualization_msgs::MarkerArray generateRefMarker(ISM::RecognitionResultPtr result, std::vector<ISM::PosePtr> voted_poses);

    std::map<ISM::ObjectPtr, std::vector<ISM::PosePtr>> getSortedVotedPoses(ISM::VotingSpacePtr voting_space_ptr);
    ISM::RecognitionResultPtr getRecognitionResultPtrOfPattern(ISM::RecognitionResultPtr result);
    visualization_msgs::Marker generateResultVoteOverlay(ISM::RecognitionResultPtr result);

    std::string base_frame_;
    double marker_lifetime_;

    std::string pattern_name_;
    std::string scene_name_;
    double bin_size_;

    double bin_scale_;
    double grid_scale_;
    double axis_scale_;
    double line_scale_;
    double object_marker_scale_;
    double bin_opacity_;
    double grid_opacity_;
    double vote_opacity_;
    double object_marker_opacity_;
    double confidence_sphere_opacity_;

    dynamic_reconfigure::Server<asr_ism_visualizations::ism_voting_visualizerConfig>* reconfigure_server_;

};
typedef boost::shared_ptr<ISMVotingVisualizerRVIZ> ISMVotingVisualizerRVIZPtr;

}