Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <swri_profiler_tools/ros_source_backend.h>
00032 #include <QCoreApplication>
00033 #include <QStringList>
00034 #include <ros/ros.h>
00035
00036 namespace swri_profiler_tools
00037 {
00038 RosSourceBackend::RosSourceBackend()
00039 :
00040 is_connected_(false)
00041 {
00042 int argc = QCoreApplication::arguments().size();
00043 std::vector<char*> argv;
00044
00045 for (const auto& arg : QCoreApplication::arguments())
00046 {
00047 auto* temp_str = new char[arg.size()];
00048 strcpy(temp_str, arg.toStdString().c_str());
00049 argv.push_back(temp_str);
00050 }
00051 ros::init(argc, &argv[0],
00052 "profiler",
00053 ros::init_options::AnonymousName);
00054 for (const auto& arg : argv)
00055 {
00056 delete[] arg;
00057 }
00058
00059 startTimer(50);
00060 }
00061
00062 RosSourceBackend::~RosSourceBackend()
00063 {
00064 if (ros::isStarted()) {
00065 ros::shutdown();
00066 ros::waitForShutdown();
00067 }
00068 }
00069
00070 void RosSourceBackend::startRos()
00071 {
00072 ros::start();
00073 is_connected_ = true;
00074
00075 ros::NodeHandle nh;
00076 index_sub_ = nh.subscribe("/profiler/index", 1000, &RosSourceBackend::handleIndex, this);
00077 data_sub_ = nh.subscribe("/profiler/data", 1000, &RosSourceBackend::handleData, this);
00078
00079 std::string uri = ros::master::getURI();
00080 Q_EMIT connected(true, QString::fromStdString(uri));
00081 }
00082
00083 void RosSourceBackend::stopRos()
00084 {
00085 ros::shutdown();
00086 is_connected_ = false;
00087 Q_EMIT connected(false, QString());
00088 }
00089
00090 void RosSourceBackend::timerEvent(QTimerEvent *event)
00091 {
00092 bool master_status = ros::master::check();
00093
00094 if (!is_connected_ && master_status) {
00095 startRos();
00096 } else if (is_connected_ && !master_status) {
00097 stopRos();
00098 } else if (is_connected_ && master_status) {
00099 ros::spinOnce();
00100 }
00101 }
00102
00103 void RosSourceBackend::handleIndex(const swri_profiler_msgs::ProfileIndexArray &msg)
00104 {
00105 Q_EMIT indexReceived(msg);
00106 }
00107
00108 void RosSourceBackend::handleData(const swri_profiler_msgs::ProfileDataArray &msg)
00109 {
00110 Q_EMIT dataReceived(msg);
00111 }
00112
00113 }