ism_result_visualizer_rviz.hpp

Go to the documentation of this file.

#pragma once
//Ilcas includes
#include <asr_ism_visualizations/visualizer_rviz.hpp>
#include <asr_ism_visualizations/VizHelperRVIZ.hpp>
#include <ISM/common_type/RecognitionResult.hpp>
#include <ISM/typedef.hpp>

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

namespace VIZ
{
class ISMResultVisualizerRVIZ : public VisualizerRVIZ
{

public:
    ISMResultVisualizerRVIZ(const ros::Publisher& publisher, const ros::NodeHandle& nh) :  VisualizerRVIZ(publisher) {
        reconfigure_server_ = new dynamic_reconfigure::Server<asr_ism_visualizations::ism_result_visualizerConfig>(nh);
        dynamic_reconfigure::Server<asr_ism_visualizations::ism_result_visualizerConfig>::CallbackType reconf_callback = boost::bind(&ISMResultVisualizerRVIZ::dynamicReconfCallback, this, _1, _2);
        reconfigure_server_->setCallback(reconf_callback);
    }

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


    void dynamicReconfCallback(asr_ism_visualizations::ism_result_visualizerConfig &config, uint32_t level) {
        ROS_DEBUG_STREAM("Result vizualizer dyn-params updated.");

        line_scale_ = config.lineScale;
        tree_depth_size_ = config.treeDepthSize;
        ism_marker_scale_ = config.ismMarkerScale;
        is_pose_relative_ = config.isPoseRelative;
        ignore_z_offset_ = config.ignoreZOffset;
        use_scene_coloring_ = config.useSceneColoring;
        use_z_max_ = config.ignoreZOffset;
        marker_lifetime_ = config.markerLifetime;
        base_frame_ = config.baseFrame;
    }

    void setSceneCount(int scene_count){
        scene_count_ = scene_count;
        scene_iterator_ = 0;

        ism_to_color_map_.clear();
        cylinder_map_.clear();
    }

    void addVisualization(const ISM::RecognitionResultPtr recognition_result);

private:
    visualization_msgs::MarkerArray genTestMarker();

    std::vector<ISM::RecognitionResultPtr> traverseTree(ISM::RecognitionResultPtr result, int depth);

    visualization_msgs::MarkerArray getMarkersFromResult(const ISM::RecognitionResultPtr result);

    ISM::PosePtr getAdjustedPose(ISM::PosePtr pose, int depth, bool is_child);
    ISM::QuaternionPtr calculateOrientation(ISM::PointPtr from_point_ptr, ISM::PointPtr to_point_ptr, bool pose_relative = false);
    ISM::PosePtr calculateCylinderPose(const ISM::PosePtr from_pose_ptr, const ISM::PointPtr to_point_ptr, double height);

    bool is_pose_relative_;
    bool ignore_z_offset_;
    double tree_depth_size_;
    double line_scale_;
    double ism_marker_scale_;
    std::string base_frame_;
    double marker_lifetime_;

    std::map<std::string, int> ism_to_depth_map_;
    std::map<std::string, ISM::PosePtr> ism_to_parent_ism_map_;
    std::map<std::string, VIZ::ColorRGBA> ism_to_color_map_;
    std::map<std::string, std::pair<ISM::PosePtr, double>> cylinder_map_;

    double z_offset_;
    bool use_z_max_;
    bool use_scene_coloring_;
    int scene_count_;
    int scene_iterator_;

    ISM::PosePtr ref_pose_;
    ISM::PointPtr last_ref_pose_;

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

};
typedef boost::shared_ptr<ISMResultVisualizerRVIZ> ISMResultVisualizerRVIZPtr;
}