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 //----------------------------------------------------------------------
00030 //----------------------------------------------------------------------
00031 
00032 
00033 // Qt Includes
00034 #include <QLineEdit>
00035 #include <QFileInfo>
00036 #include <QFileDialog>
00037 #include <QStringList>
00038 #include <QStandardItemModel>
00039 #include <QModelIndex>
00040 
00041 // Own Includes
00042 #include "rqt_marble_plugin/marble_plugin.h"
00043 
00044 // ROS Plugin Includes
00045 #include <pluginlib/class_list_macros.h>
00046 
00047 
00048 // Marble Includes
00049 #include <marble/MarbleWidget.h>
00050 #include <marble/MarbleModel.h>
00051 #include <marble/MapThemeManager.h>
00052 #include <marble/GeoPainter.h>
00053 
00054 #include <ros/package.h>
00055 #include "drawable_marble_widget.h"
00056 #include "manage_kml_dialog.h"
00057 
00058 // @TODO: setDistance does not work on reloading
00059 // @TODO: ComboBox for the MarbleWidget projection method
00060 // @TOOD: Draw icon on the current gps pos (MarbleWidget needs to be subclassed (custom paint))
00061 
00062 namespace rqt_marble_plugin {
00063 
00064 MarblePlugin::MarblePlugin()
00065   : rqt_gui_cpp::Plugin()
00066   , widget_(0)
00067 {
00068   // give QObjects reasonable names
00069   setObjectName("MarbleWidgetPlugin");
00070 }
00071 
00072 
00073 
00074 void MarblePlugin::initPlugin(qt_gui_cpp::PluginContext& context)
00075 {
00076   // access standalone command line arguments
00077   QStringList argv = context.argv();
00078   qRegisterMetaType<FlyToMode>( "FlyToMode" );
00079   qRegisterMetaType<GeoDataLookAt>( "GeoDataLookAt" );
00080 
00081   // create QWidget
00082   widget_ = new QWidget();
00083 
00084   // add widget to the user interface
00085   ui_.setupUi( widget_ );
00086 
00087   ui_.MarbleWidget->setMapThemeId("earth/openstreetmap/openstreetmap.dgml");
00088   ui_.MarbleWidget->setProjection( Marble::Mercator );
00089   //ui_.MarbleWidget->centerOn( 115.87164 , -31.93452 , false );  // My Happy Place: The Scotto
00090   ui_.MarbleWidget->centerOn( 8.426, 49.01, false );  
00091   ui_.MarbleWidget->setDistance(0.05);
00092 
00093   context.addWidget(widget_);
00094   ui_.comboBox_theme->setModel( m_map_theme_manager.mapThemeModel() );
00095 
00096 
00097   //set refresh icon
00098   QIcon refresh_icon;
00099   std::string path = ros::package::getPath("rqt_marble_plugin")+"/etc/refresh.png";
00100   QString icon_path(path.c_str());
00101   refresh_icon.addFile(icon_path);
00102   ui_.refreshButton->setIcon(refresh_icon);
00103 
00104   //setup the ros publisher for publishing the selected gps position
00105   m_selected_gps_pos_publisher = getNodeHandle().advertise< sensor_msgs::NavSatFix >("gps_position", 1);
00106 
00107   //subscribe to the visualization topic
00108   if(ui_.checkBox_process_marker->isChecked())
00109     subscribeVisualization();
00110 
00111   m_mapcontrol_subscriber = getNodeHandle().subscribe< geometry_msgs::Twist >( "/mapcontrol" , 1 , &MarblePlugin::mapcontrolCallback, this );
00112 
00113   FindNavSatFixTopics();
00114 
00115   // Connections
00116   connect(ui_.comboBox_current_gps, SIGNAL(activated (const QString &)), this, SLOT (ChangeGPSTopicCurrentGPS(const QString &)));
00117   connect(ui_.comboBox_matched_gps, SIGNAL(activated (const QString &)), this, SLOT (ChangeGPSTopicMatchedGPS(const QString &)));
00118 
00119 
00120   connect(ui_.checkBox_process_marker, SIGNAL(clicked()), this, SLOT(processMarkerCheckBoxCLicked()));
00121   connect(ui_.refreshButton, SIGNAL(clicked()), this, SLOT(FindNavSatFixTopics()));
00122   connect(ui_.manageKMLButton, SIGNAL(clicked()), this, SLOT(ManageKML()));
00123 
00124   connect( this , SIGNAL(NewGPSPosition(qreal,qreal)) , ui_.MarbleWidget , SLOT(centerOn(qreal,qreal)) );
00125 
00126   connect(this, SIGNAL(ZoomIn(FlyToMode)), ui_.MarbleWidget, SLOT(zoomIn(FlyToMode)));
00127   connect(this, SIGNAL(ZoomOut(FlyToMode)), ui_.MarbleWidget, SLOT(zoomOut(FlyToMode)));
00128   connect(this, SIGNAL(flyTo(GeoDataLookAt,FlyToMode)), ui_.MarbleWidget, SLOT(flyTo(GeoDataLookAt,FlyToMode)));
00129 
00130   connect( ui_.comboBox_theme , SIGNAL(currentIndexChanged(int)) , this , SLOT(ChangeMarbleModelTheme(int)));
00131 
00132   connect( ui_.MarbleWidget, SIGNAL(mouseClickGeoPosition(qreal,qreal,GeoDataCoordinates::Unit)), this, SLOT(gpsCoordinateSelected(qreal,qreal,GeoDataCoordinates::Unit)));
00133 
00134   // AutoNavigation Connections ... soon
00135   /*
00136   m_autoNavigation = new Marble::AutoNavigation( ui_.MarbleWidget->model(), ui_.MarbleWidget->viewport(), this );
00137 
00138   connect( m_autoNavigation, SIGNAL( zoomIn( FlyToMode ) ),
00139                        ui_.MarbleWidget, SLOT( zoomIn() ) );
00140   connect( m_autoNavigation, SIGNAL( zoomOut( FlyToMode ) ),
00141                        ui_.MarbleWidget, SLOT( zoomOut() ) );
00142   connect( m_autoNavigation, SIGNAL( centerOn( const GeoDataCoordinates &, bool ) ),
00143                        ui_.MarbleWidget, SLOT( centerOn( const GeoDataCoordinates & ) ) );
00144 
00145   connect( ui_.MarbleWidget , SIGNAL( visibleLatLonAltBoxChanged() ),
00146                         m_autoNavigation, SLOT( inhibitAutoAdjustments() ) );
00147     */
00148 }
00149 
00150 void MarblePlugin::ManageKML()
00151 {
00152     ManageKmlDialog kmlDialog(m_last_kml_data);
00153     if(kmlDialog.exec() == QDialog::Accepted)
00154     {
00155         std::map< QString, bool> kml_files = kmlDialog.getKmlFiles();
00156         addKMLData(kml_files, true);
00157     }
00158 }
00159 
00160 void MarblePlugin::addKMLData(std::map< QString, bool>& kml_files, bool overwrite)
00161 {
00162     if(overwrite)
00163     {
00164         clearKMLData();
00165     }
00166 
00167     std::map<QString, bool>::iterator it;
00168     for(it=kml_files.begin(); it != kml_files.end(); it++)
00169     {
00170         QString filepath = it->first;
00171         bool show = it->second;
00172 
00173         if(show)
00174         {
00175             ui_.MarbleWidget->model()->addGeoDataFile( filepath );
00176         }
00177         m_last_kml_data[filepath] = show;
00178     }
00179 }
00180 
00181 void MarblePlugin::mapcontrolCallback(const geometry_msgs::TwistConstPtr &msg)
00182 {
00183     FlyToMode mode;
00184     mode = Linear;
00185 
00186     GeoDataLookAt lookAt;
00187     lookAt = ui_.MarbleWidget->lookAt();
00188 
00189     double lon = lookAt.longitude();
00190     double lat = lookAt.latitude();
00191 
00192     double muted_move_step = 0.025 * ui_.MarbleWidget->moveStep();
00193     float step_lon = muted_move_step * std::max(std::min(msg->linear.x, 1.0), -1.0);
00194     float step_lat = muted_move_step * std::max(std::min(msg->linear.y, 1.0), -1.0);
00195 
00196     lon+=step_lon;
00197     lat+=step_lat;
00198 
00199     if(lon!=lookAt.longitude() || lat!=lookAt.latitude())
00200     {
00201         lookAt.setLatitude(lat);
00202         lookAt.setLongitude(lon);
00203 
00204         emit flyTo(lookAt, mode);
00205     }
00206 
00207     if(msg->linear.z < 0)
00208         emit ZoomIn(mode);
00209     if(msg->linear.z > 0)
00210         emit ZoomOut(mode);
00211 
00212 }
00213 
00214 void MarblePlugin::clearKMLData()
00215 {
00216     for(std::map< QString, bool>::iterator it = m_last_kml_data.begin(); it != m_last_kml_data.end(); it++)
00217     {
00218         ui_.MarbleWidget->model()->removeGeoData(it->first);
00219     }
00220     m_last_kml_data.clear();
00221 }
00222 
00223 
00224 
00225 void MarblePlugin::FindNavSatFixTopics()
00226 {
00227     using namespace ros::master;
00228 
00229     std::vector<TopicInfo> topic_infos;
00230     getTopics(topic_infos);
00231 
00232     //GPS Topics
00233     ui_.comboBox_current_gps->clear();
00234     ui_.comboBox_matched_gps->clear();
00235     for(std::vector<TopicInfo>::iterator it=topic_infos.begin(); it!=topic_infos.end();it++)
00236     {
00237         TopicInfo topic = (TopicInfo)(*it);
00238         if(topic.datatype.compare("sensor_msgs/NavSatFix")==0)
00239         {
00240             QString lineEdit_string(topic.name.c_str());
00241             ui_.comboBox_current_gps->addItem(lineEdit_string);
00242             ui_.comboBox_matched_gps->addItem(lineEdit_string);
00243         }
00244     }
00245 }
00246 
00247 
00248 void MarblePlugin::ChangeMarbleModelTheme(int idx )
00249 {
00250     QStandardItemModel* model = m_map_theme_manager.mapThemeModel();
00251     QModelIndex index = model->index( idx , 0 );
00252     QString theme = model->data( index , Qt::UserRole+1  ).toString();
00253 
00254     ui_.MarbleWidget->setMapThemeId( theme );
00255 }
00256 
00257 void MarblePlugin::ChangeGPSTopicCurrentGPS(const QString &topic_name)
00258 {
00259     m_current_pos_subscriber.shutdown();
00260     m_current_pos_subscriber = getNodeHandle().subscribe< sensor_msgs::NavSatFix >(
00261                 topic_name.toStdString().c_str() , 1 , &MarblePlugin::GpsCallbackCurrent, this );
00262 
00263     int idx = ui_.comboBox_current_gps->findText( topic_name );
00264     if( idx != -1 )
00265     {
00266         ui_.comboBox_current_gps->setCurrentIndex( idx );
00267     }
00268 }
00269 
00270 void MarblePlugin::ChangeGPSTopicMatchedGPS(const QString &topic_name)
00271 {
00272     m_matched_pos_subscriber.shutdown();
00273     m_matched_pos_subscriber = getNodeHandle().subscribe< sensor_msgs::NavSatFix >(
00274                 topic_name.toStdString().c_str() , 1 , &MarblePlugin::GpsCallbackMatched, this );
00275 
00276     int idx = ui_.comboBox_matched_gps->findText( topic_name );
00277     if( idx != -1 )
00278     {
00279         ui_.comboBox_matched_gps->setCurrentIndex( idx );
00280     }
00281 
00282 }
00283 
00284 void MarblePlugin::GpsCallbackMatched( const sensor_msgs::NavSatFixConstPtr& gpspt )
00285 {
00286     // std::cout << "GPS Callback Matched " << gpspt->longitude << " " << gpspt->latitude << std::endl;
00287     assert( widget_ );
00288 
00289     GeoDataCoordinates postition(gpspt->longitude, gpspt->latitude, gpspt->altitude, GeoDataCoordinates::Degree);
00290     ui_.MarbleWidget->setMatchedPosition(postition);
00291 
00292     // Emit NewGPSPosition only, if it changes significantly. Has to be somehow related to the zoom
00293     static qreal _x = -1;
00294     static qreal _y = -1;
00295 
00296     qreal x;
00297     qreal y;
00298 
00299     ui_.MarbleWidget->screenCoordinates(gpspt->longitude,gpspt->latitude , x , y );
00300 
00301     bool recenter = ui_.checkBox_center->isChecked();
00302 
00303     // Recenter if lat long within <threshold> pixels away from center
00304     qreal threshold = 20;
00305     recenter &=  ((x - _x) * (x - _x) + (y - _y) * (y - _y)) > threshold;
00306 
00307     if( recenter )
00308     {
00309         ui_.MarbleWidget->screenCoordinates(gpspt->longitude,gpspt->latitude , _x , _y );
00310         emit NewGPSPosition( gpspt->longitude , gpspt->latitude );
00311     }
00312 }
00313 
00314 void MarblePlugin::GpsCallbackCurrent( const sensor_msgs::NavSatFixConstPtr& gpspt )
00315 {
00316     // std::cout << "GPS Callback Current " << gpspt->longitude << " " << gpspt->latitude << std::endl;
00317     assert( widget_ );
00318 
00319     GeoDataCoordinates postition(gpspt->longitude, gpspt->latitude, gpspt->altitude, GeoDataCoordinates::Degree);
00320     ui_.MarbleWidget->setCurrentPosition(postition);
00321 
00322     // set reference point for marker calculation
00324     ui_.MarbleWidget->referenceGpsCallback(gpspt);
00325 
00326     // @TODO: Marble Widget does not repaint
00327 }
00328 
00329 void MarblePlugin::gpsCoordinateSelected(qreal lon, qreal lat, GeoDataCoordinates::Unit unit) {
00330   GeoDataCoordinates coords(lon, lat, unit);
00331 
00332   if(ui_.checkBox_publish_gps->isChecked())
00333   {
00334     sensor_msgs::NavSatFix msg;
00335     msg.longitude = coords.longitude(GeoDataCoordinates::Degree);
00336     msg.latitude = coords.latitude(GeoDataCoordinates::Degree);
00337 
00338     m_selected_gps_pos_publisher.publish(msg);
00339   }
00340 }
00341 
00342 void MarblePlugin::processMarkerCheckBoxCLicked()
00343 {
00344   if(ui_.checkBox_process_marker->isChecked())
00345   {
00346     subscribeVisualization();
00347   }
00348   else
00349   {
00350     m_visualization_subscriber.shutdown();
00351     m_visualization_marker_array_subscriber.shutdown();
00352     m_reference_gps_subscriber.shutdown();
00353   }
00354 }
00355 
00356 void MarblePlugin::subscribeVisualization()
00357 {
00358   m_visualization_subscriber = getNodeHandle().subscribe("/visualization_marker", 1, &DrawableMarbleWidget::visualizationCallback, ui_.MarbleWidget);
00359   m_visualization_marker_array_subscriber = getNodeHandle().subscribe("/visualization_marker_array", 1, &DrawableMarbleWidget::visualizationMarkerArrayCallback, ui_.MarbleWidget);
00360 //  m_reference_gps_subscriber = getNodeHandle().subscribe("/intersection_gps_position", 1, &DrawableMarbleWidget::referenceGpsCallback, ui_.MarbleWidget);
00361 }
00362 
00363 void MarblePlugin::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const
00364 {
00365   // save intrinsic configuration, usually using:
00366     QString topic(m_current_pos_subscriber.getTopic().c_str());
00367     instance_settings.setValue( "marble_plugin_topic_current", topic );
00368 
00369     topic = QString(m_matched_pos_subscriber.getTopic().c_str());
00370     instance_settings.setValue( "marble_plugin_topic_matched", topic );
00371 
00372     instance_settings.setValue( "marble_plugin_zoom" , ui_.MarbleWidget->distance() );
00373     instance_settings.setValue( "marble_theme_index" , ui_.comboBox_theme->currentIndex() );
00374     instance_settings.setValue( "marble_center" , ui_.checkBox_center->isChecked() );
00375     instance_settings.setValue( "piblish_gps" , ui_.checkBox_publish_gps->isChecked() );
00376     instance_settings.setValue( "process_marker" , ui_.checkBox_process_marker->isChecked() );
00377 
00378 
00379     //save kml files
00380     int i=0;
00381     instance_settings.setValue("kml_number", QVariant::fromValue(m_last_kml_data.size()) );
00382     for(std::map< QString, bool>::const_iterator it = m_last_kml_data.begin(); it != m_last_kml_data.end(); it++)
00383     {
00384         QString key("kml_file_");
00385         key.append(i);
00386         instance_settings.setValue(key, it->first);
00387 
00388         //TODO: save visibility. Another way must be foud here
00389 //        key.append("_show");
00390 //        instance_settings.setValue(key, it->second);
00391         i++;
00392     }
00393 }
00394 
00395 void MarblePlugin::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings)
00396 {
00397   // restore intrinsic configuration, usually using:
00398     const QString topic_current = instance_settings.value("marble_plugin_topic_current").toString();
00399     ChangeGPSTopicCurrentGPS(topic_current);
00400 
00401     const QString topic_matched = instance_settings.value("marble_plugin_topic_matched").toString();
00402     ChangeGPSTopicMatchedGPS(topic_matched);
00403 
00404     ui_.comboBox_theme->setCurrentIndex( instance_settings.value( "marble_theme_index" , 0 ).toInt() );
00405     ui_.checkBox_center->setChecked( instance_settings.value( "marble_center" , true ).toBool());
00406     ui_.checkBox_publish_gps->setChecked( instance_settings.value( "piblish_gps" , true ).toBool());
00407     ui_.checkBox_process_marker->setChecked( instance_settings.value( "process_marker" , true ).toBool());
00408 
00409 
00410     //load kml files
00411     std::map< QString, bool> kml_files;
00412     int number = instance_settings.value("kml_number",0).toInt();
00413     for(int i=0; i<number; i++)
00414     {
00415         QString key("kml_file_");
00416         key.append(i);
00417         kml_files[instance_settings.value(key,0).toString()] = true;
00418     }
00419     addKMLData(kml_files, true);
00420 
00421 
00422   // std::cout << "Set distance " << instance_settings.value( "marble_plugin_zoom" ).toReal() << std::endl;
00423 
00424   // @TODO: Does not work since the KML loading changes the zoom
00425   ui_.MarbleWidget->setDistance( instance_settings.value( "marble_plugin_zoom" , 0.05 ).toReal() );
00426 }
00427 
00428 void MarblePlugin::shutdownPlugin()
00429 {
00430   // unregister all publishers here
00431   m_current_pos_subscriber.shutdown();
00432   m_matched_pos_subscriber.shutdown();
00433   m_selected_gps_pos_publisher.shutdown();
00434   m_mapcontrol_subscriber.shutdown();
00435   m_visualization_subscriber.shutdown();
00436   m_visualization_marker_array_subscriber.shutdown();
00437   m_reference_gps_subscriber.shutdown();
00438 }
00439 
00440 /*bool hasConfiguration() const
00441 {
00442   return true;
00443 }
00444 
00445 void triggerConfiguration()
00446 {
00447   // Usually used to open a dialog to offer the user a set of configuration
00448 }*/
00449 
00450 } // namespace
00451 PLUGINLIB_EXPORT_CLASS( rqt_marble_plugin::MarblePlugin , rqt_gui_cpp::Plugin )


marble_plugin
Author(s): Tobias Bär
autogenerated on Thu Aug 27 2015 13:56:10