KinematicChainVisualizer.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 <Pose.h>
#include <ISM/common_type/Pose.hpp>

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

namespace Visualization
{
  class KinematicChainVisualizer : public AbstractExtendedVisualizer
  {
  public:
    
    KinematicChainVisualizer();
    
    ~KinematicChainVisualizer();
    
    void publishObjectPositionAsArrow(boost::shared_ptr<ros::Publisher> pPublisher,
                                      unsigned int& pMarkerId,
                      const boost::shared_ptr<ISM::Pose> pPose,
                                      double pQualityOfMatch);
    
    void publishObjectPositionAsPoint(boost::shared_ptr<ros::Publisher> pPublisher,
                                      unsigned int& pMarkerId,
                      const boost::shared_ptr<ISM::Pose> pPose);
    
    void publishObjectPositionAsPointWithScore(boost::shared_ptr<ros::Publisher> pPublisher,
                                               unsigned int& pMarkerId,
                           const boost::shared_ptr<ISM::Pose> pPose,
                                               std::vector<double> pScores,
                                               double pQualityOfMatch);
    
    void publishLink(boost::shared_ptr<ros::Publisher> pPublisher,
                     unsigned int& pMarkerId,
                     const Eigen::Vector3d pFrom,
                     const Eigen::Vector3d pTo,
                     double pQualityOfMatch);
    
    void setBestStatus(bool pStatus);
    
  private:
    
    visualization_msgs::Marker generatePointMessage(unsigned int& pMarkerId,
                                                    const Eigen::Vector3d pPosition);
    
    visualization_msgs::Marker generateArrowMessage(unsigned int& pMarkerId,
                                                    const Eigen::Vector3d pFrom,
                                                    const Eigen::Vector3d pTo,
                                                    double pQualityOfMatch,
                                                    double pScalePeak,
                                                    double pScaleShaft,
                                                    double pPeakLength);
    
    visualization_msgs::Marker generatePerpendicularArrowMessage(unsigned int& pMarkerId,
                                 const boost::shared_ptr<ISM::Pose> pPose,
                                                                 double pQualityOfMatch, double pLength);
    
    visualization_msgs::Marker generateTermIndicatorMessage(unsigned int& pMarkerId,
                                const boost::shared_ptr<ISM::Pose> pPose,
                                                            double pOffset,
                                                            double pScore);
    
  private:
    
    bool mBestStatus;
  };
}