plot.h

Go to the documentation of this file.

#ifndef PLOT_H
#define PLOT_H

#include <deque>
#include <map>

#include <dynamic_reconfigure/server.h>
#include <gtest/gtest.h>
#include <message_filters/subscriber.h>
#include <ros/ros.h>
#include <rviz_visual_tools/rviz_visual_tools.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_listener.h>

#include <toposens_driver/sensor.h>
#include <toposens_markers/TsMarkersConfig.h>
#include <toposens_msgs/TsScan.h>

namespace toposens_markers
{
static const std::string kMarkersTopic = "ts_markers";
static const std::string kMarkersNs = "TsMarkers";
static const std::string kMeshNs = "TsSensor";

class Plot
{
  static const auto _baseScale = rviz_visual_tools::scales::LARGE;

public:
  Plot(ros::NodeHandle nh, ros::NodeHandle private_nh);
  ~Plot();

  void publishSensorMeshes();

  void publishDefaultSensorMesh();

private:
  typedef dynamic_reconfigure::Server<TsMarkersConfig> Cfg;

  void _reconfig(TsMarkersConfig &cfg, uint32_t level);

  void _plot(const toposens_msgs::TsScan::ConstPtr &msg);

  void _staticTFCallback(const tf2_msgs::TFMessage::ConstPtr &tf);

  std::string _frame_id;     
  double _lifetime;          
  int _color_mode;           
  float _color_range;        
  float _half_color_range;   
  std::shared_ptr<Cfg> _srv; 
  std::deque<toposens_msgs::TsScan> _scans;    
  rviz_visual_tools::RvizVisualToolsPtr _rviz; 
  std::string _target_frame; 
  message_filters::Subscriber<toposens_msgs::TsScan>
      _tf2_scans_sub;                        
  tf2_ros::Buffer _tf2_buffer;               
  tf2_ros::TransformListener *_tf2_listener; 
  tf2_ros::MessageFilter<toposens_msgs::TsScan> *_tf2_filter; 
  ros::Subscriber _scans_sub;     
  std::string _sensor_mesh;       
  ros::Subscriber _static_tf_sub; 
  std::map<std::string, geometry_msgs::Pose> _pose_map;  
  std::map<std::string, std_msgs::ColorRGBA> _color_map; 
  std::map<std::string, rviz_visual_tools::colors> _mesh_color_map;
};

}  // namespace toposens_markers

#endif