00001 #ifndef DATASTREAM_ROS_TOPIC_H 00002 #define DATASTREAM_ROS_TOPIC_H 00003 00004 #include <QtPlugin> 00005 #include <QAction> 00006 #include <thread> 00007 #include <topic_tools/shape_shifter.h> 00008 #include "PlotJuggler/datastreamer_base.h" 00009 #include <ros_type_introspection/ros_introspection.hpp> 00010 00011 00012 class DataStreamROS: public QObject, DataStreamer 00013 { 00014 Q_OBJECT 00015 Q_PLUGIN_METADATA(IID "com.icarustechnology.PlotJuggler.DataStreamer" "../datastreamer.json") 00016 Q_INTERFACES(DataStreamer) 00017 00018 public: 00019 00020 DataStreamROS(); 00021 00022 virtual PlotDataMap &getDataMap() override; 00023 00024 virtual bool start(QString& default_configuration) override; 00025 00026 virtual void shutdown() override; 00027 00028 virtual void enableStreaming(bool enable) override; 00029 00030 virtual bool isStreamingEnabled() const override; 00031 00032 virtual ~DataStreamROS() override; 00033 00034 virtual const char* name() const override { return "ROS Topic Streamer"; } 00035 00036 virtual void setParentMenu(QMenu* menu) override; 00037 00038 private: 00039 00040 void topicCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string &topic_name); 00041 00042 void updateLoop(); 00043 00044 PlotDataMap _plot_data; 00045 00046 bool _enabled; 00047 bool _running; 00048 00049 std::thread _thread; 00050 00051 void extractInitialSamples(); 00052 00053 double _initial_time; 00054 bool _use_header_timestamp; 00055 00056 ros::NodeHandlePtr _node; 00057 std::vector<ros::Subscriber> _subscribers; 00058 RosIntrospection::SubstitutionRuleMap _rules; 00059 00060 int _received_msg_count; 00061 00062 QAction* _action_saveIntoRosbag; 00063 00064 std::map<std::string, int> _msg_index; 00065 00066 private slots: 00067 00068 void saveIntoRosbag(); 00069 }; 00070 00071 #endif // DATALOAD_CSV_H