record_visualizer_rviz.cpp

Go to the documentation of this file.

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

namespace VIZ
{
void RecordVisualizerRVIZ::addVisualization(const ISM::TracksPtr record)
{
    visualization_msgs::MarkerArray current_marker_array = RecordVisualizerRVIZ::generateRecordMarkers(record);
    addMarker(current_marker_array);
    publishCollectedMarkers();
}

visualization_msgs::MarkerArray RecordVisualizerRVIZ::generateRecordMarkers(const ISM::TracksPtr record)
{
    visualization_msgs::MarkerArray return_markers;
    double color_step_size = 240.0 / (record->tracks.size() + 1);

    /* create actual marker for each track */
    std::vector<ISM::PosePtr> track_poses;
    std_msgs::ColorRGBA track_color;
    visualization_msgs::MarkerArray temp_markers;
    std::string marker_namespace;
    for (size_t i = 0; i < record->tracks.size(); i++)
    {
        track_poses = VizHelperRVIZ::posesFromTrack(record->tracks[i]);
        track_color = VizHelperRVIZ::hsvToRGBA(color_step_size * (i + 1), 1.0, 1.0);
        marker_namespace = this->prefix_ + record->tracks[i]->type + "_" + record->tracks[i]->observedId + "_track";

        temp_markers = VizHelperRVIZ::createTrackMarkers(track_poses, this->base_frame_, marker_namespace, 0.006, track_color, this->marker_lifetime_);
        return_markers.markers.insert(return_markers.markers.end(), temp_markers.markers.begin(), temp_markers.markers.end());
    }

    return return_markers;
}
}