SampleVisualizer.h

Go to the documentation of this file.

# pragma once

// Global includes
#include <map>
#include <vector>

// Package includes
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

#include <tf/LinearMath/Vector3.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <tf/LinearMath/Transform.h>
#include <tf/transform_datatypes.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <Pose.h>
#include <ISM/common_type/Pose.hpp>

// Local includes3
#include "visualization/psm/helper/AbstractExtendedVisualizer.h"

namespace Visualization
{
  static const unsigned int VISUALIZATION_OFFSET = 1;
  
  class SampleVisualizer : public AbstractExtendedVisualizer
  {
  public:
    
    SampleVisualizer();
    
    ~SampleVisualizer();
    
    void setParentPose(boost::shared_ptr<ISM::Pose> pPose);
    
    void publishTrajectory(boost::shared_ptr<ros::Publisher> pPublisher,
                           unsigned int& pMarkerId,
                           const std::vector<Eigen::Vector3d>& pSamples);
    
    void publishTrajectory(boost::shared_ptr<ros::Publisher> mPublisher,
                           unsigned int& pMarkerId,
                           const std::vector<Eigen::Vector3d>& pAbsoluteSample,
                           const std::vector<Eigen::Vector3d>& pAbsoluteParentSample);
  private:
    
    visualization_msgs::Marker generateTrajectoryMarker(unsigned int& pMarkerId, const std::vector<Eigen::Vector3d>& pSamples);
    
    visualization_msgs::Marker generateArrowMessage(unsigned int& pMarkerId, Eigen::Vector3d pFrom, Eigen::Vector3d pTo);
    
  private:
    
    boost::shared_ptr<ISM::Pose> mParentPose;
  };
}