00001 #ifndef DATASTREAM_ROS_TOPIC_H 00002 #define DATASTREAM_ROS_TOPIC_H 00003 00004 #include <QtPlugin> 00005 #include <QAction> 00006 #include <QTimer> 00007 #include <thread> 00008 #include <topic_tools/shape_shifter.h> 00009 #include "PlotJuggler/datastreamer_base.h" 00010 #include <ros_type_introspection/ros_introspection.hpp> 00011 #include <rosgraph_msgs/Clock.h> 00012 #include "../dialog_select_ros_topics.h" 00013 #include "../RosMsgParsers/ros_parser.h" 00014 00015 class DataStreamROS: public DataStreamer 00016 { 00017 Q_OBJECT 00018 Q_PLUGIN_METADATA(IID "com.icarustechnology.PlotJuggler.DataStreamer" "../datastreamer.json") 00019 Q_INTERFACES(DataStreamer) 00020 00021 public: 00022 00023 DataStreamROS(); 00024 00025 virtual bool start(QStringList* selected_datasources) override; 00026 00027 virtual void shutdown() override; 00028 00029 virtual bool isRunning() const override; 00030 00031 virtual ~DataStreamROS() override; 00032 00033 virtual const char* name() const override { return "ROS Topic Streamer"; } 00034 00035 virtual bool xmlSaveState(QDomDocument &doc, QDomElement &parent_element) const override; 00036 00037 virtual bool xmlLoadState(const QDomElement &parent_element ) override; 00038 00039 virtual void addActionsToParentMenu( QMenu* menu ) override; 00040 00041 virtual std::vector<QString> appendData(PlotDataMapRef& destination) override 00042 { 00043 _destination_data = &destination; 00044 return DataStreamer::appendData(destination); 00045 } 00046 00047 private: 00048 00049 PlotDataMapRef* _destination_data; 00050 00051 void topicCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string &topic_name); 00052 00053 void clockCallback(const rosgraph_msgs::Clock::ConstPtr& msg); 00054 00055 void extractInitialSamples(); 00056 00057 void timerCallback(); 00058 00059 void subscribe(); 00060 00061 void saveDefaultSettings(); 00062 00063 void loadDefaultSettings(); 00064 00065 bool _running; 00066 00067 std::shared_ptr<ros::AsyncSpinner> _spinner; 00068 00069 double _initial_time; 00070 00071 std::string _prefix; 00072 00073 ros::NodeHandlePtr _node; 00074 00075 std::map<std::string, ros::Subscriber> _subscribers; 00076 00077 RosIntrospection::SubstitutionRuleMap _rules; 00078 00079 int _received_msg_count; 00080 00081 QAction* _action_saveIntoRosbag; 00082 00083 std::map<std::string, int> _msg_index; 00084 00085 DialogSelectRosTopics::Configuration _config; 00086 00087 RosMessageParser _ros_parser; 00088 00089 QTimer* _periodic_timer; 00090 00091 bool _roscore_disconnection_already_notified; 00092 00093 double _prev_clock_time; 00094 00095 private: 00096 00097 static void saveIntoRosbag(const PlotDataMapRef& data); 00098 }; 00099 00100 #endif // DATALOAD_CSV_H