marble_plugin.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 /* -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 
00005  Copyright (c) 2013, TB
00006  All rights reserved.
00007 
00008  Redistribution and use in source and binary forms are permitted
00009  provided that the above copyright notice and this paragraph are
00010  duplicated in all such forms and that any documentation,
00011  advertising materials, and other materials related to such
00012  distribution and use acknowledge that the software was developed
00013  by TB. The name of the
00014  TB may not be used to endorse or promote products derived
00015  from this software without specific prior written permission.
00016  THIS SOFTWARE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
00017  IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
00018  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00019 
00020  -- END LICENSE BLOCK ----------------------------------------------*/
00021 
00022 //----------------------------------------------------------------------
00029 //----------------------------------------------------------------------
00030 #include <marble/MarbleWidget.h>
00031 #include <marble/MarbleModel.h>
00032 #include <marble/MapThemeManager.h>
00033 #include <marble/GeoPainter.h>
00034 #include <marble/GeoDataCoordinates.h>
00035 #include <marble/Route.h>
00036 #include <marble/RoutingModel.h>
00037 // Qt Includes
00038 #include <QLineEdit>
00039 #include <QFileInfo>
00040 #include <QFileDialog>
00041 #include <QStringList>
00042 #include <QStandardItemModel>
00043 #include <QModelIndex>
00044 
00045 #include <pluginlib/class_list_macros.h>
00046 #include <ros/package.h>
00047 
00048 #include "rqt_marble/marble_plugin.h"
00049 #include "rqt_marble/bridge_ros_marble.h"
00050 
00051 // @TODO: setDistance does not work on reloading
00052 // @TODO: ComboBox for the MarbleWidget projection method
00053 // @TOOD: Draw icon on the current gps pos (MarbleWidget needs to be subclassed (custom paint))
00054 
00055 namespace rqt_marble
00056 {
00057 
00058 MarblePlugin::MarblePlugin() :
00059     rqt_gui_cpp::Plugin(), widget_(0)
00060 {
00061   // give QObjects reasonable names
00062   setObjectName("MarbleWidgetPlugin");
00063 
00064   this->ros_navigation = new BridgeRosMarble();
00065 
00066   ROS_INFO("in constructor");
00067 }
00068 
00072 void MarblePlugin::initPlugin(qt_gui_cpp::PluginContext& context)
00073 {
00074   ROS_INFO("in initPlugin");
00075 
00076   // access standalone command line arguments
00077   QStringList argv = context.argv();
00078 
00079   initWidget(context);
00080 
00081   this->findGpsTopics();
00082 }
00083 
00084 void MarblePlugin::initWidget(qt_gui_cpp::PluginContext& context)
00085 {
00086 
00087   // create QWidget
00088   widget_ = new QWidget();
00089 
00090   // add widget to the user interface
00091   ui_.setupUi(widget_);
00092   ui_.marble_widget->setMapThemeId("earth/openstreetmap/openstreetmap.dgml");
00093   ui_.marble_widget->setProjection(Marble::Mercator);
00094   //ui_.marble_widget->centerOn(115.87164, -31.93452, false); // My Happy Place: The Scotto
00095   ui_.marble_widget->centerOn(-122.0795, 37.4000, false); // OSRF
00096   ui_.marble_widget->setDistance(0.05);
00097 
00098   context.addWidget(widget_);
00099   map_theme_manager = new Marble::MapThemeManager(widget_);
00100   ui_._combobox_theme->setModel(map_theme_manager->mapThemeModel());
00101 
00102   QIcon refresh_icon; //set refresh icon
00103   std::string path = ros::package::getPath("rqt_marble") + "/etc/refresh.png";
00104   QString icon_path(path.c_str());
00105   refresh_icon.addFile(icon_path);
00106   ui_._gpstopic_refresh_button->setIcon(refresh_icon);
00107 
00108   //Trying to set proportion on splitter; Map should be much larger that the
00109   // control pane. But not really sure if the values in the argument are
00110   // appropriate.
00111   ui_._splitter_h_top->setStretchFactor(0, 20);
00112 
00113   // Connections
00114   connect(ui_._gpstopic_combobox, SIGNAL(activated (const QString &)), this,
00115           SLOT (changeGpsTopic(const QString &)));
00116   connect(ui_._gpstopic_refresh_button, SIGNAL(clicked()), this,
00117           SLOT(findGpsTopics()));
00118   connect(this, SIGNAL(newGpsPosition(qreal, qreal)), ui_.marble_widget,
00119           SLOT(centerOn(qreal, qreal)));
00120 //  connect( ui_.lineEdit_topic , SIGNAL(editingFinished()) , this , SLOT( changeGpsTopic()) );
00121   connect(ui_.lineEdit_kml, SIGNAL(returnPressed()), this, SLOT(setKmlFile()));
00122   connect(ui_._combobox_theme, SIGNAL(currentIndexChanged(int)), this,
00123           SLOT(changeMarbleModelTheme(int)));
00124   connect(ui_._checkbox_navigation, SIGNAL(clicked(bool)), this,
00125           SLOT(enableNavigation(bool)));
00126 
00127 // AutoNavigation Connections ... soon
00128 //  m_autoNavigation = new Marble::AutoNavigation(ui_.marble_widget->model(), ui_.ui_.marble_widget->viewport(), this);
00129 //
00130 //  connect(m_autoNavigation, SIGNAL( zoomIn( FlyToMode ) ), ui_.marble_widget, SLOT( zoomIn() ));
00131 //  connect(m_autoNavigation, SIGNAL( zoomOut( FlyToMode ) ), ui_.marble_widget, SLOT( zoomOut() ));
00132 //  connect(m_autoNavigation, SIGNAL( centerOn( const GeoDataCoordinates &, bool ) ), ui_.marble_widget,
00133 //          SLOT( centerOn( const GeoDataCoordinates & ) ));
00134 //  connect(ui_.marble_widget, SIGNAL( visibleLatLonAltBoxChanged() ), m_autoNavigation, SLOT( inhibitAutoAdjustments() ));
00135 
00136   //For GPS Topic emission
00137   routing_manager = ui_.marble_widget->model()->routingManager();
00138   this->routeModel = routing_manager->routingModel();
00139   connect(this->routeModel, SIGNAL(currentRouteChanged()), this,
00140           SLOT(routeChanged()));
00141 }
00142 
00143 void MarblePlugin::findGpsTopics()
00144 {
00145   using namespace ros::master;
00146   std::vector<TopicInfo> topic_infos;
00147   getTopics(topic_infos);
00148 
00149   ui_._gpstopic_combobox->clear();
00150   for (std::vector<TopicInfo>::iterator it = topic_infos.begin();
00151       it != topic_infos.end(); it++)
00152   {
00153     TopicInfo topic = (TopicInfo)(*it);
00154     if (topic.datatype.compare("sensor_msgs/NavSatFix") == 0)
00155     {
00156 // std::cout << "found " << topic.name << std::endl;
00157       QString lineEdit_string(topic.name.c_str());
00158       ui_._gpstopic_combobox->addItem(lineEdit_string);
00159     }
00160   }
00161 }
00162 
00163 void MarblePlugin::shutdownPlugin()
00164 {
00165 // unregister all publishers here
00166   m_sat_nav_fix_subscriber.shutdown();
00167 }
00168 
00169 void MarblePlugin::changeMarbleModelTheme(int idx)
00170 {
00171   QStandardItemModel* model =
00172       map_theme_manager->mapThemeModel();
00173   QModelIndex index = model->index(idx, 0);
00174   QString theme = model->data(index, Qt::UserRole + 1).toString();
00175 
00176   ui_.marble_widget->setMapThemeId(theme);
00177 }
00178 
00179 void MarblePlugin::changeGpsTopic(const QString &topic_name)
00180 {
00181   m_sat_nav_fix_subscriber.shutdown();
00182   m_sat_nav_fix_subscriber = getNodeHandle().subscribe<sensor_msgs::NavSatFix>(
00183       topic_name.toStdString().c_str(), 10, &MarblePlugin::gpsCallback, this);
00184 }
00185 
00186 void MarblePlugin::setKmlFile(bool envoke_file_dialog)
00187 {
00188   QFileInfo fi(ui_.lineEdit_kml->text());
00189 
00190   if (!fi.isFile() && envoke_file_dialog)
00191   {
00192     QString fn = QFileDialog::getOpenFileName(0, tr("Open Geo Data File"),
00193                                               tr(""),
00194                                               tr("Geo Data Files (*.kml)"));
00195     fi.setFile(fn);
00196   }
00197 
00198   if (fi.isFile())
00199   {
00200     ui_.marble_widget->model()->addGeoDataFile(fi.absoluteFilePath());
00201 
00202     ui_.lineEdit_kml->setText(fi.absoluteFilePath());
00203   }
00204   else
00205   {
00206     ui_.lineEdit_kml->setText("");
00207   }
00208 }
00209 
00210 void MarblePlugin::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gpspt)
00211 {
00212 // std::cout << "GPS Callback " << gpspt->longitude << " " << gpspt->latitude << std::endl;
00213   assert(widget_);
00214 
00215 // Emit newGpsPosition only, if it changes significantly. Has to be somehow related to the zoom
00216   static qreal _x = -1;
00217   static qreal _y = -1;
00218 
00219   qreal x;
00220   qreal y;
00221 
00222 // Recenter if lat long is not on screen
00223   bool recenter = !ui_.marble_widget->screenCoordinates(gpspt->longitude,
00224                                                         gpspt->latitude, x, y);
00225   recenter |= ui_._checkBox_centering->isChecked();
00226 
00227 // Recenter if lat long within <threshold> pixels away from center
00228   qreal threshold = 20;
00229   recenter |= ((x - _x) * (x - _x) + (y - _y) * (y - _y)) > threshold;
00230 
00231   if (recenter)
00232   {
00233     emit newGpsPosition(gpspt->longitude, gpspt->latitude);
00234     ui_.marble_widget->screenCoordinates(gpspt->longitude, gpspt->latitude, _x,
00235                                          _y);
00236   }
00237 }
00238 
00239 void MarblePlugin::saveSettings(qt_gui_cpp::Settings& plugin_settings,
00240                                 qt_gui_cpp::Settings& instance_settings) const
00241 {
00242 // save intrinsic configuration, usually using:
00243   QString topic(m_sat_nav_fix_subscriber.getTopic().c_str());
00244   instance_settings.setValue("rqt_marble_topic", topic);
00245   instance_settings.setValue(
00246       "rqt_marble_kml_file",
00247       ui_.lineEdit_kml->text().replace(".", "___dot_replacement___"));
00248   instance_settings.setValue("rqt_marble_zoom", ui_.marble_widget->distance());
00249   instance_settings.setValue("marble_theme_index",
00250                              ui_._combobox_theme->currentIndex());
00251   instance_settings.setValue("marble_center",
00252                              ui_._checkBox_centering->isChecked());
00253 }
00254 
00255 void MarblePlugin::restoreSettings(
00256     const qt_gui_cpp::Settings& plugin_settings,
00257     const qt_gui_cpp::Settings& instance_settings)
00258 {
00259 // restore intrinsic configuration, usually using:
00260   const QString topic = instance_settings.value("rqt_marble_topic").toString();
00261   changeGpsTopic(topic);
00262 
00263   ui_.lineEdit_kml->setText(
00264       instance_settings.value("rqt_marble_kml_file", "").toString().replace(
00265           "___dot_replacement___", "."));
00266   ui_._combobox_theme->setCurrentIndex(
00267       instance_settings.value("marble_theme_index", 0).toInt());
00268   ui_._checkBox_centering->setChecked(
00269       instance_settings.value("marble_center", true).toBool());
00270 
00271 // std::cout << "Set distance " << instance_settings.value( "rqt_marble_zoom" ).toReal() << std::endl;
00272 
00273   setKmlFile(false);
00274 
00275 // @TODO: Does not work since the KML loading changes the zoom
00276   ui_.marble_widget->setDistance(
00277       instance_settings.value("rqt_marble_zoom", 0.05).toReal());
00278 }
00279 
00280 /*bool hasConfiguration() const
00281  {
00282  return true;
00283  }
00284 
00285  void triggerConfiguration()
00286  {
00287  // Usually used to open a dialog to offer the user a set of configuration
00288  }*/
00289 
00290 void MarblePlugin::enableNavigation(bool checked)
00291 {
00292   if (checked)
00293     this->ros_navigation->setDoNavigation(true);
00294   else
00295     this->ros_navigation->setDoNavigation(false);
00296 }
00297 
00298 void MarblePlugin::routeChanged()
00299 {
00300   Marble::Route route = this->routeModel->route();
00301   this->ros_navigation->publishRouteInGps(route);
00302 }
00303 
00304 } // namespace
00305 
00306 PLUGINLIB_EXPORT_CLASS(rqt_marble::MarblePlugin, rqt_gui_cpp::Plugin)


rqt_marble
Author(s): Isaac Saito , Tobias Baer , Jan Aidel
autogenerated on Mon Oct 6 2014 07:15:54