drawable_marble_widget.cpp
Go to the documentation of this file.
00001 #include "rqt_marble_plugin/drawable_marble_widget.h"
00002 
00003 #include "ros/package.h"
00004 #include "iostream"
00005 #include "math.h"
00006 
00007 #include <boost/foreach.hpp>
00008 
00009 using namespace rqt_marble_plugin;
00010 
00011 
00012 DrawableMarbleWidget::DrawableMarbleWidget(QWidget *parent)
00013   : MarbleWidget(parent),
00014     m_current_pos(M_PI_2,M_PI_2),
00015     m_matched_pos(M_PI_2,M_PI_2),
00016     m_ref_lat(49.021267),
00017     m_ref_lon(8.3983866)
00018 {
00019   std::string path;
00020   path = ros::package::getPath("rqt_marble_plugin")+"/etc/arrow.png";
00021   loadImage(m_arrow, path);
00022 
00023   path = ros::package::getPath("rqt_marble_plugin")+"/etc/sattelite.png";
00024   loadImage(m_current_pos_icon, path);
00025 
00026   path = ros::package::getPath("rqt_marble_plugin")+"/etc/matched.png";
00027   loadImage(m_matched, path);
00028 }
00029 
00030 void DrawableMarbleWidget::loadImage( QImage& icon , std::string& path )
00031 {
00032   QString qpath(path.c_str());
00033   QImage image(qpath);
00034   icon = image;
00035 }
00036 
00037 
00038 
00039 void DrawableMarbleWidget::customPaint(Marble::GeoPainter *painter)
00040 {
00041   MarbleWidget::customPaint( painter );
00042 
00043   // @TODO: Check if both points are available
00044 
00045   if( ! m_current_pos.isPole() )
00046   {
00047     painter->drawImage( m_current_pos , m_current_pos_icon );
00048   }
00049 
00050   if( ! m_matched_pos.isPole() )
00051   {
00052     painter->drawImage( m_matched_pos , roateCar( &m_arrow ) );
00053   }
00054 
00055   if( ! ( m_current_pos.isPole() || m_matched_pos.isPole() ) )
00056   {
00057     painter->save();
00058     painter->setPen( QPen(Qt::red  , 2 ) );
00059     GeoDataLineString lineString;
00060     lineString << m_matched_pos << m_current_pos;
00061     painter->drawPolyline(lineString);
00062     painter->restore();
00063   }
00064 
00065   painter->setPen(QPen(Qt::blue, 2));
00066 
00067   for(std::map<std::string, PolygonSet>::iterator set_it = m_marker_line.begin(); set_it != m_marker_line.end();set_it++)
00068   {
00069     std::list<ColoredPolygon> polygons = set_it->second.polygons;
00070     for (std::list<ColoredPolygon>::iterator poly_it = polygons.begin(); poly_it!= polygons.end(); poly_it++)
00071     {
00072       //set color
00073       QColor lineColor(Qt::blue);
00074       std_msgs::ColorRGBA color_msg = poly_it->color;
00075       getColor(lineColor, color_msg);
00076 
00077       //draw line
00078       painter->setPen(QPen(lineColor, 2));
00079       painter->drawPolyline(poly_it->polygon);
00080     }
00081 
00082   }
00083 
00084   for(std::map<std::string, CircleSet>::iterator set_it = m_marker_circle.begin(); set_it != m_marker_circle.end();set_it++)
00085   {
00086     std::list<Circle> circles = set_it->second.circles;
00087 
00088     for (std::list<Circle>::iterator circle_it = circles.begin(); circle_it!= circles.end(); circle_it++)
00089     {
00090       //set color
00091       QColor color(Qt::blue);
00092       std_msgs::ColorRGBA color_msg = circle_it->color;
00093       getColor(color, color_msg);
00094 
00095       //draw line
00096       painter->setPen(QPen(color, 2));
00097       painter->setBrush(QBrush(color, Qt::SolidPattern));
00098       painter->drawEllipse(circle_it->mid, circle_it->r, circle_it->r, true);
00099     }
00100   }
00101 }
00102 
00103 void DrawableMarbleWidget::getColor(QColor& outputColor, std_msgs::ColorRGBA color_msg)
00104 {
00105   outputColor.setRed(255*color_msg.r);
00106   outputColor.setGreen(255*color_msg.g);
00107   outputColor.setBlue(255*color_msg.b);
00108   outputColor.setAlpha(255*color_msg.a);
00109 }
00110 
00111 std::string DrawableMarbleWidget::getMarkerId(visualization_msgs::Marker marker)
00112 {
00113   std::stringstream id;
00114   id << marker.ns<<"_"<<marker.id;
00115   return id.str();
00116 }
00117 
00118 void DrawableMarbleWidget::removeOldCircles(const ros::Time& actual_time)
00119 {
00120   for(std::map<std::string, CircleSet>::iterator set_it = m_marker_circle.begin(); set_it != m_marker_circle.end();set_it++)
00121   {
00122     CircleSet set = set_it->second;
00123     if(set.lifetime.toNSec() > 0 && set.creation_time + set.lifetime > actual_time)
00124       m_marker_circle.erase(set_it->first);
00125 
00126   }
00127 }
00128 
00129 void DrawableMarbleWidget::removeOldPolygons(const ros::Time& actual_time)
00130 {
00131   for(std::map<std::string, PolygonSet>::iterator set_it = m_marker_line.begin(); set_it != m_marker_line.end();set_it++)
00132   {
00133     PolygonSet set = set_it->second;
00134     if(set.lifetime.toNSec() > 0 && set.creation_time + set.lifetime > actual_time)
00135       m_marker_line.erase(set_it->first);
00136   }
00137 }
00138 
00139 void DrawableMarbleWidget::setMatchedPosition( GeoDataCoordinates &postion )
00140 {
00141   m_last_matched_position = m_matched_pos;
00142   m_matched_pos = postion;
00143 }
00144 
00145 void DrawableMarbleWidget::setCurrentPosition( GeoDataCoordinates &postion )
00146 {
00147   m_current_pos = postion;
00148 }
00149 
00150 QImage DrawableMarbleWidget::roateCar(QImage *car_image)
00151 {
00152   double x1 = m_last_matched_position.latitude(GeoDataCoordinates::Radian);
00153   double x2 = m_matched_pos.latitude(GeoDataCoordinates::Radian);
00154 
00155   double y1 = m_last_matched_position.longitude(GeoDataCoordinates::Radian);
00156   double y2 = m_matched_pos.longitude(GeoDataCoordinates::Radian);
00157 
00158   static double alpha = atan2( y2 - y1 , x2 - x1 ) * 180 * M_1_PI;
00159   if(posChanged( x1 , y1 , x2 , y2 ,  1.0e-9 ) )
00160   {
00161     alpha = atan2( y2 - y1 , x2 - x1 ) * 180 * M_1_PI;
00162   }
00163 
00164   QTransform rotation;
00165   rotation.rotate( alpha );
00166   return car_image->transformed( rotation );
00167 }
00168 
00169 bool DrawableMarbleWidget::posChanged(double x1, double y1, double x2, double y2, double threshold)
00170 {
00171   double diff = std::abs(std::max(x1-x2, y1-y2));
00172   return  diff > threshold;
00173 }
00174 
00175 std::pair<double, double> DrawableMarbleWidget::toGpsCoordinates(double x, double y)
00176 {
00177   return GetAbsoluteCoordinates(x, y, m_ref_lat, m_ref_lon, -M_PI_2);
00178 }
00179 
00180 
00181 void DrawableMarbleWidget::visualizationMarkerArrayCallback(const visualization_msgs::MarkerArrayConstPtr &markers)
00182 {
00183   BOOST_FOREACH(visualization_msgs::Marker marker, markers->markers)
00184   {
00185     addMarker(marker);
00186   }
00187 }
00188 
00189 void DrawableMarbleWidget::visualizationCallback(const visualization_msgs::MarkerConstPtr &marker) {
00190   addMarker(*marker);
00191 }
00192 
00193 void DrawableMarbleWidget::addLineStrip(const visualization_msgs::Marker &marker)
00194 {
00195   PolygonSet poly_set;
00196   poly_set.creation_time = marker.header.stamp;
00197   poly_set.lifetime = marker.lifetime;
00198 
00199   GeoDataLineString geo_polygon;
00200   for (size_t i=0; i<marker.points.size(); i++) {
00201     std::pair<double, double> coords = toGpsCoordinates(marker.points.at(i).x, marker.points.at(i).y);
00202 
00203     GeoDataCoordinates geo_coords;
00204     geo_coords.set(coords.second, coords.first, GeoDataCoordinates::Degree, GeoDataCoordinates::Degree);
00205     geo_polygon.append(geo_coords);
00206   }
00207 
00208   ColoredPolygon polygon;
00209   polygon.polygon = geo_polygon;
00210   polygon.color = marker.color;
00211 
00212   poly_set.polygons.push_back(polygon);
00213 
00214   m_marker_line[getMarkerId(marker)] = poly_set;
00215 }
00216 
00217 void DrawableMarbleWidget::addLineList(const visualization_msgs::Marker &marker)
00218 {
00219   PolygonSet poly_set;
00220   poly_set.creation_time = marker.header.stamp;
00221   poly_set.lifetime = marker.lifetime;
00222 
00223   //read out points and create a line
00224   for (size_t i=0; i<marker.points.size()-1; i+=2) {
00225     std::pair<double, double> coords1 = toGpsCoordinates(marker.points.at(i).x, marker.points.at(i).y);
00226     std::pair<double, double> coords2 = toGpsCoordinates(marker.points.at(i+1).x, marker.points.at(i+1).y);
00227 
00228     GeoDataCoordinates geo_coords1;
00229     geo_coords1.set(coords1.second, coords1.first, GeoDataCoordinates::Degree, GeoDataCoordinates::Degree);
00230     GeoDataCoordinates geo_coords2;
00231     geo_coords2.set(coords2.second, coords2.first, GeoDataCoordinates::Degree, GeoDataCoordinates::Degree);
00232     GeoDataLineString geo_polygon;
00233     geo_polygon.append(geo_coords1);
00234     geo_polygon.append(geo_coords2);
00235 
00236     ColoredPolygon polygon;
00237     polygon.polygon = geo_polygon;
00238     polygon.color = marker.color;
00239 
00240     poly_set.polygons.push_back(polygon);
00241   }
00242 
00243   m_marker_line[getMarkerId(marker)] = poly_set;
00244 }
00245 
00246 void DrawableMarbleWidget::addSphereList(const visualization_msgs::Marker &marker)
00247 {
00248   CircleSet circle_set;
00249   circle_set.creation_time = marker.header.stamp;
00250   circle_set.lifetime = marker.lifetime;
00251 
00252   for (size_t i=0; i<marker.points.size(); i++) {
00253     std::pair<double, double> coords = toGpsCoordinates(marker.points.at(i).x, marker.points.at(i).y);
00254     GeoDataCoordinates geo_coords;
00255     geo_coords.set(coords.second, coords.first, GeoDataCoordinates::Degree, GeoDataCoordinates::Degree);
00256 
00257     Circle circle;
00258     circle.mid = geo_coords;
00259     circle.r = 0.00002*marker.scale.x;
00260     circle.color = marker.color;
00261 
00262     circle_set.circles.push_back(circle);
00263   }
00264 
00265   m_marker_circle[getMarkerId(marker)] = circle_set;
00266 }
00267 
00268 void DrawableMarbleWidget::addSphere(const visualization_msgs::Marker &marker)
00269 {
00270   CircleSet circle_set;
00271   circle_set.creation_time = marker.header.stamp;
00272   circle_set.lifetime = marker.lifetime;
00273 
00274   std::pair<double, double> coords = toGpsCoordinates(marker.pose.position.x, marker.pose.position.y);
00275   GeoDataCoordinates geo_coords;
00276   geo_coords.set(coords.second, coords.first, GeoDataCoordinates::Degree, GeoDataCoordinates::Degree);
00277 
00278   Circle circle;
00279   circle.mid = geo_coords;
00280   circle.r = 0.00002*marker.scale.x;
00281   circle.color = marker.color;
00282 
00283   circle_set.circles.push_back(circle);
00284 
00285 
00286   m_marker_circle[getMarkerId(marker)] = circle_set;
00287 }
00288 
00289 void DrawableMarbleWidget::addMarker(const visualization_msgs::Marker &marker)
00290 {
00291   //save the marker in the right data structure, so customPaint() can use it to paint
00292 
00293   removeOldPolygons(marker.header.stamp);
00294   removeOldCircles(marker.header.stamp);
00295 
00296   if(marker.action == visualization_msgs::Marker::ADD)
00297   {
00298     switch (marker.type)
00299     {
00300       case visualization_msgs::Marker::LINE_STRIP:
00301       {
00302         addLineStrip(marker);
00303         break;
00304       }
00305 
00306       case visualization_msgs::Marker::LINE_LIST:
00307       {
00308         addLineList(marker);
00309         break;
00310       }
00311       case visualization_msgs::Marker::CUBE:
00313         break;
00314       case visualization_msgs::Marker::SPHERE_LIST:
00315       {
00316         addSphereList(marker);
00317         break;
00318       }
00319       case visualization_msgs::Marker::SPHERE:
00320       {
00321         addSphere(marker);
00322         break;
00323       }
00324     }
00325   }
00326   else if (marker.action == visualization_msgs::Marker::DELETE)
00327   {
00328     std::string index = getMarkerId(marker);
00329     m_marker_circle.erase(index);
00330     m_marker_line.erase(index);
00331   }
00332 }
00333 
00334 void DrawableMarbleWidget::referenceGpsCallback(const sensor_msgs::NavSatFixConstPtr &reference)
00335 {
00336   m_ref_lat = reference->latitude;
00337   m_ref_lon = reference->longitude;
00338 }
00339 
00340 // --- From Gps Tools
00341 
00342 std::pair<double, double> DrawableMarbleWidget::GetAbsoluteCoordinates( double x , double y , double ref_lat , double ref_lon, double ref_bearing)
00343 {
00344   double d = sqrt(x*x+y*y);
00345   double bearing = atan2(x,y) + ref_bearing;
00346 
00347   return GetNewPointBearingDistance(ref_lat, ref_lon, bearing, d);
00348 }
00349 
00350 std::pair<double, double> DrawableMarbleWidget::GetNewPointBearingDistance(double a_lat, double a_lon, double bearing, double distance)
00351 {
00352   const double R = 6371000; //Earth radius in m
00353 
00354   a_lat = a_lat /180 * M_PI;
00355   a_lon = a_lon /180 * M_PI;
00356 
00357   double b_lat = asin( sin(a_lat)*cos(distance/R) + cos(a_lat)*sin(distance/R)*cos(bearing) );
00358   double b_lon = a_lon + atan2(sin(bearing)*sin(distance/R)*cos(a_lat), cos(distance/R)-sin(a_lat)*sin(b_lat));
00359 
00360   b_lat = b_lat * 180 / M_PI;
00361   b_lon = b_lon * 180 / M_PI;
00362 
00363   return std::make_pair(b_lat, b_lon);
00364 
00365 }


marble_plugin
Author(s): Tobias Bär
autogenerated on Mon Oct 6 2014 02:13:12