ObjectModelVisualizerRVIZ.cpp

Go to the documentation of this file.

//Header includes
#include <asr_ism_visualizations/ObjectModelVisualizerRVIZ.hpp>

//Ilcas includes
#include <asr_ism_visualizations/VizHelperRVIZ.hpp>
#include <ISM/recognizer/VotingSpace.hpp>
#include <ISM/utility/Util.hpp>
#include <ISM/utility/GeometryHelper.hpp>

namespace VIZ
{



void  ObjectModelVisualizerRVIZ::drawObjectModels(const std::vector<ISM::ObjectPtr>& objects, const std::map<ISM::ObjectPtr, double>& objects_to_hue_map)
{
    visualization_msgs::MarkerArray vizObjects;
    vizObjects = ObjectModelVisualizerRVIZ::getMarkersFromObjects(objects, objects_to_hue_map);
    addMarker(vizObjects);
    publishCollectedMarkers();
}

visualization_msgs::MarkerArray ObjectModelVisualizerRVIZ::getMarkersFromObjects(const std::vector<ISM::ObjectPtr> objects, const std::map<ISM::ObjectPtr, double>& objects_to_hue_map)
{
    visualization_msgs::MarkerArray retMarkers;

    std::string markerNamespace = this->prefix + "Object_Mesh_";
    int id = 0;

    for(const ISM::ObjectPtr& object : objects)
    {
        std::string tempPath;
        tempPath = object->ressourcePath.string();
        if( !tempPath.empty() )
        {
            visualization_msgs::Marker tempObjectMarker;
            tempObjectMarker = VizHelperRVIZ::createMeshMarker(object->pose, this->baseFrame, markerNamespace + object->type + object->observedId, 0,
                                                               VizHelperRVIZ::getColorOfObject(object), this->markerLifetime,
                                                               tempPath);

            // predefined highlighting exists?
            std::map<ISM::ObjectPtr, double>::const_iterator it = objects_to_hue_map.find(object);
            if (it != objects_to_hue_map.end())
            {
                tempObjectMarker.color = VizHelperRVIZ::hsvToRGBA(it->second, 1.0, 1.0);
                tempObjectMarker.color.a = 0.5;
            }


            retMarkers.markers.push_back(tempObjectMarker);
        } else
        {
            visualization_msgs::MarkerArray tempMarkers;
            tempMarkers = VizHelperRVIZ::createCoordinateMarker(object->pose, this->baseFrame, markerNamespace + object->type + object->observedId + "_FRAME" ,
                                                                id, 0.05, 0.002, this->markerLifetime);


            retMarkers.markers.insert(retMarkers.markers.end(), tempMarkers.markers.begin(), tempMarkers.markers.end());
            retMarkers.markers.insert(retMarkers.markers.end(), tempMarkers.markers.begin(), tempMarkers.markers.end());
            id += tempMarkers.markers.size();
        }
        //Visualize Orientation-Axis(X-Axis)
        visualization_msgs::Marker axisMarker = VIZ::VizHelperRVIZ::createMarkerWithoutTypeAndPose(baseFrame, markerNamespace + object->type + object->observedId + "_xAXIS", id++, 0.1, 0.01, 0.01, VIZ::VizHelperRVIZ::createColorRGBA(1, 0, 0, 1), markerLifetime);
        axisMarker.type = visualization_msgs::Marker::ARROW;
        Eigen::Quaterniond transformed_X_Axis = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) * ISM::GeometryHelper::quatToEigenQuat(object->pose->quat);
        axisMarker.pose.orientation = VizHelperRVIZ::quatToQuaternionMsg(ISM::GeometryHelper::eigenQuatToQuat(transformed_X_Axis));
        axisMarker.pose.position = VizHelperRVIZ::pointToPointMsg(object->pose->point);
        axisMarker.action = visualization_msgs::Marker::ADD;
        retMarkers.markers.push_back(axisMarker);
    }

    return retMarkers;
}
}