PosePredictionVisualizerRVIZ.cpp

Go to the documentation of this file.

//Header include
#include <asr_ism_visualizations/PosePredictionVisualizerRVIZ.hpp>

//Ilcas Include
#include <asr_ism_visualizations/VizHelperRVIZ.hpp>

namespace VIZ {

void PosePredictionVisualizer::addPosePredictionVisualization(ISM::PosePtr referencePosePtr,
                                                              asr_msgs::AsrAttributedPointCloud attributedPointCloud,
                                                              std::string markerNameSpace)
{
    ROS_DEBUG("Publish Debug Marker");
    std::string referenceNameSpace = markerNameSpace + "_reference";
    std::string arrowNamespace = markerNameSpace + "_arrows";
    std::string pointCloudNamespace = markerNameSpace + "_point_cloud";

    ColorRGBA referenceColor = VizHelperRVIZ::createColorRGBA(0.0, 0.0, 1.0, 1.0);
    ColorRGBA pointColor = VizHelperRVIZ::createColorRGBA(0.0, 1.0, 0.0, 1.0);
    unsigned int id = 0;

    Marker referenceMarker = VizHelperRVIZ::createSphereMarker(referencePosePtr->point,
                                                               baseFrame, referenceNameSpace, id, referenceRadius,
                                                               referenceColor, arrowLifeTime);
    id++;
    addMarker(referenceMarker);

    std::map< std::string, ColorRGBA> objectColors;
    for (asr_msgs::AsrAttributedPoint attributedPoint: attributedPointCloud.elements)
    {
        std::string object_type = attributedPoint.type;
        ColorRGBA arrowColor;
        if (objectColors.find(object_type) != objectColors.end())
            arrowColor = objectColors[object_type];
        else
        {
            arrowColor = VizHelperRVIZ::hsvToRGBA(colorStepSize * objectColors.size(), 1.0, 1.0);
            objectColors[object_type] = arrowColor;
        }

        ISM::PointPtr ismPointPtr(new ISM::Point(attributedPoint.pose.position.x,
                                                 attributedPoint.pose.position.y,
                                                 attributedPoint.pose.position.z));
        Marker arrowMarker = VizHelperRVIZ::createArrowMarkerToPoint(referencePosePtr->point,ismPointPtr,
                                                                     baseFrame, arrowNamespace, id++, arrowScaleX,
                                                                     arrowScaleY, arrowScaleZ, arrowColor,
                                                                     arrowLifeTime);
        Marker pointCloudMarker = VizHelperRVIZ::createSphereMarker(ismPointPtr, baseFrame,
                                                                    pointCloudNamespace, id++ , pointRadius,
                                                                    pointColor, pointLifeTime);
        addMarker(arrowMarker);
        addMarker(pointCloudMarker);
    }
}
}