vote_visualizer_rviz.cpp

Go to the documentation of this file.

//Local include
#include <asr_ism_visualizations/vote_visualizer_rviz.hpp>
#include <asr_ism_visualizations/VizHelperRVIZ.hpp>

//ISM includes
#include <ISM/utility/GeometryHelper.hpp>

namespace VIZ
{
void VoteVisualizerRVIZ::addVisualization(const std::map<ISM::ObjectPtr, std::vector<ISM::VoteSpecifierPtr>>& object_to_votes)
{
    visualization_msgs::MarkerArray current_marker_array = generateVoteMarkers(object_to_votes);

    addMarker(current_marker_array);
    publishCollectedMarkers();
}

visualization_msgs::MarkerArray VoteVisualizerRVIZ::generateVoteMarkers(const std::map<ISM::ObjectPtr, std::vector<ISM::VoteSpecifierPtr>>& object_to_votes)
{
    visualization_msgs::MarkerArray return_markers;

    int i = 0;
    for (std::map<ISM::ObjectPtr, std::vector<ISM::VoteSpecifierPtr>>::const_iterator obj_to_votes_it = object_to_votes.begin(); obj_to_votes_it != object_to_votes.end(); ++obj_to_votes_it)
    {
        visualization_msgs::Marker marker;
        std::string name_space = prefix_ + obj_to_votes_it->first->type + "_" + obj_to_votes_it->first->observedId;
        double hue_step = 360 / object_to_votes.size();
        double hue_value = hue_step * i++;

        int id = 0;
        for (const ISM::VoteSpecifierPtr& vote : obj_to_votes_it->second)
        {
            ISM::PointPtr dest = ISM::GeometryHelper::applyQuatAndRadiusToPose(obj_to_votes_it->first->pose, vote->objectToRefQuat, vote->radius);
            marker = VizHelperRVIZ::createArrowMarkerToPoint(obj_to_votes_it->first->pose->point, dest, base_frame_, name_space + "_votes", id++, 0.0015, 0.003, 0.003, VizHelperRVIZ::hsvToRGBA(hue_value, 1.0, 1.0), 0, 0.003);
            return_markers.markers.push_back(marker);
        }
    }

    return return_markers;
}
}