PosePredictionVisualizerRVIZ.hpp

Go to the documentation of this file.

#pragma once
//Ilcas includes
#include <asr_ism_visualizations/visualizer_rviz.hpp>
#include <ISM/common_type/Pose.hpp>
#include <asr_msgs/AsrAttributedPointCloud.h>

//ROS includes
#include <dynamic_reconfigure/server.h>
#include <asr_ism_visualizations/rp_pose_prediction_visualizerConfig.h>

//foreigen includes
#include <boost/shared_ptr.hpp>

namespace VIZ
{
class PosePredictionVisualizer: public VisualizerRVIZ
{
public:
    PosePredictionVisualizer(ros::Publisher posePredictionMarkerPublisher, const ros::NodeHandle& nh): VisualizerRVIZ(posePredictionMarkerPublisher) {
        reconfigure_server_ = new dynamic_reconfigure::Server<asr_ism_visualizations::rp_pose_prediction_visualizerConfig>(nh);
        dynamic_reconfigure::Server<asr_ism_visualizations::rp_pose_prediction_visualizerConfig>::CallbackType reconf_callback = boost::bind(&PosePredictionVisualizer::dynamicReconfCallback, this, _1, _2);
        reconfigure_server_->setCallback(reconf_callback);
    }

    ~PosePredictionVisualizer(){
        reconfigure_server_->clearCallback();
        delete reconfigure_server_;
    }

    void addPosePredictionVisualization(ISM::PosePtr referencePosePtr, asr_msgs::AsrAttributedPointCloud attributedPointCloud, std::string markerNameSpace);

    void dynamicReconfCallback(asr_ism_visualizations::rp_pose_prediction_visualizerConfig &config, uint32_t level) {
        ROS_DEBUG_STREAM("Pose prediction visualizer dyn-params updated.");
        baseFrame = config.baseFrame;
        pointLifeTime = config.pointLifeTime;
        arrowLifeTime = config.arrowLifeTime;
        referenceRadius = config.referenceRadius;
        pointRadius = config.pointRadius;
        arrowScaleX = config.arrowScaleX;
        arrowScaleY = config.arrowScaleY;
        arrowScaleZ = config.arrowScaleZ;
        colorStepSize = config.colorStepSize;

    }

private:
    typedef visualization_msgs::MarkerArray MarkerArray;
    ros::Publisher posePredictionMarkerPublisher;
    std::string baseFrame;
    double pointLifeTime;
    double arrowLifeTime;
    double referenceRadius;
    double pointRadius;
    double arrowScaleX;
    double arrowScaleY;
    double arrowScaleZ;
    double colorStepSize;

    dynamic_reconfigure::Server<asr_ism_visualizations::rp_pose_prediction_visualizerConfig>* reconfigure_server_;

};

typedef boost::shared_ptr<PosePredictionVisualizer> PosePredictionVisualizerPtr;
}