コード例 #1
0
  bool UAVFlightPlan::exportToKMLFile(const std::string &f, int begin_, int end_) const {
    bool ret_val = true;
    
    if (end_ < 0 || end_ > size()) {
      end_ = size(); 
    }
  
    if (begin_ < 0 || begin_ > size()) {
      begin_ = 0;
    }
    
    const_iterator it = begin();
    
    // Advance to the first waypoint to save
    int i = 0;
    for (; i < begin_; i++, it++);

    KmlFactory* factory = KmlFactory::GetFactory();
    
    
		kmldom::DocumentPtr doc = factory->CreateDocument();
		
    for (int j = 1; i < end_ && it != end(); j++,i++, it++) {
			ostringstream name_;
			name_ << "Waypoint " << j;
			// Create a <Point> with <coordinates> from the given Vec3.
			kmlbase::Vec3 v(it->getLongitude(), it->getLatitude(), it->getAltitude());
			kmldom::PointPtr point = kmlconvenience::CreatePointFromVec3(v);
			PlacemarkPtr place = factory->CreatePlacemark();
			place->set_geometry(point);
			doc->add_feature(place);
    }
    
    
    
    // Finally create the kml
    KmlPtr kml = factory->CreateKml();
    kml->set_feature(doc);
    
    // Then the file
    KmlFilePtr kmlfile = KmlFile::CreateFromImport(kml);
    if (!kmlfile) {
      cerr << "error: could not create kml file" << endl;
      return false;
    }
    
    // And write it
    std::string kml_data;
    kmlfile->SerializeToString(&kml_data);
    if (!kmlbase::File::WriteStringToFile(kml_data, f.c_str())) {
      cerr << "error: write of " << f << " failed" << endl;
      ret_val = false;
    }
    
    return ret_val;
  }
コード例 #2
0
ファイル: kmlexport.cpp プロジェクト: 1heinz/TauLabs
/**
 * @brief KmlExport::createTimespanPlacemark Creates a timespan placemark, which allows the
 * trajectory to be played forward in time. The placemark also contains pertinent data about
 * the vehicle's state at that timespan
 * @param timestampPoint
 * @param lastPlacemarkTime
 * @param newPlacemarkTime
 * @return Returns the placemark containing the timespan
 */
PlacemarkPtr KmlExport::createTimespanPlacemark(const LLAVCoordinates &timestampPoint, quint32 lastPlacemarkTime, quint32 newPlacemarkTime)
{
    // Create coordinates
    CoordinatesPtr coordinates = factory->CreateCoordinates();
    coordinates->add_latlngalt(timestampPoint.latitude, timestampPoint.longitude, timestampPoint.altitude);

    // Create point, using previous coordinates
    PointPtr point = factory->CreatePoint();
    point->set_extrude(true); // Extrude to ground
    point->set_altitudemode(kmldom::ALTITUDEMODE_ABSOLUTE);
    point->set_coordinates(coordinates);

    // Create the timespan
    TimeSpanPtr timeSpan = factory->CreateTimeSpan();
    QDateTime startTime = QDateTime::currentDateTimeUtc().addMSecs(lastPlacemarkTime); // FIXME: Make it a function of the realtime preferably gotten from the GPS
    QDateTime endTime = QDateTime::currentDateTimeUtc().addMSecs(newPlacemarkTime);
    timeSpan->set_begin(startTime.toString(dateTimeFormat).toStdString());
    timeSpan->set_end(endTime.toString(dateTimeFormat).toStdString());

    // Create an icon style. This arrow icon will be rotated and colored to represent velocity
    AttitudeActual::DataFields attitudeActualData = attitudeActual->getData();
    AirspeedActual::DataFields airspeedActualData = airspeedActual->getData();
    IconStylePtr iconStyle = factory->CreateIconStyle();
    iconStyle->set_color(mapVelocity2Color(airspeedActualData.CalibratedAirspeed));
    iconStyle->set_heading(attitudeActualData.Yaw + 180); //Adding 180 degrees because the arrow art points down, i.e. south.

    // Create a line style. This defines the style for the "legs" connecting the points to the ground.
    LineStylePtr lineStyle = factory->CreateLineStyle();
    lineStyle->set_color(mapVelocity2Color(timestampPoint.groundspeed));

    // Link the style to the icon
    StylePtr style = factory->CreateStyle();
    style->set_linestyle(lineStyle);
    style->set_iconstyle(iconStyle);

    // Generate the placemark with all above attributes
    PlacemarkPtr placemark = factory->CreatePlacemark();
    placemark->set_geometry(point);
    placemark->set_timeprimitive(timeSpan);
    placemark->set_name(QString("%1").arg(timeStamp / 1000.0).toStdString());
    placemark->set_visibility(true);

    // Set the placemark to use the custom rotated arrow style
    placemark->set_styleurl("#directiveArrowStyle");
    placemark->set_styleselector(style);

    // Add a nice description to the placemark
    placemark->set_description(informationString.toStdString());

    return placemark;
}
コード例 #3
0
ファイル: kmlexport.cpp プロジェクト: 1heinz/TauLabs
/**
 * @brief KmlExport::CreateLineStringPlacemark Adds a line segment which is colored according to the
 * vehicle's speed.
 * @param startPoint Beginning point along line
 * @param endPoint End point point along line
 * @return Returns the placemark containing the line segment
 */
PlacemarkPtr KmlExport::CreateLineStringPlacemark(const LLAVCoordinates &startPoint, const LLAVCoordinates &endPoint, quint32 newPlacemarkTime)
{
    CoordinatesPtr coordinates = factory->CreateCoordinates();
    coordinates->add_latlngalt(startPoint.latitude, startPoint.longitude, startPoint.altitude);
    coordinates->add_latlngalt(endPoint.latitude,   endPoint.longitude,   endPoint.altitude);

    LineStringPtr linestring = factory->CreateLineString();
    linestring->set_extrude(true); // Extrude to ground
    linestring->set_altitudemode(kmldom::ALTITUDEMODE_ABSOLUTE);
    linestring->set_coordinates(coordinates);

    StyleMapPtr styleMap = factory->CreateStyleMap();


    // Add custom balloon style (gets rid of "Directions to here...")
    // https://groups.google.com/forum/?fromgroups#!topic/kml-support-getting-started/2CqF9oiynRY
    BalloonStylePtr balloonStyle = factory->CreateBalloonStyle();
    balloonStyle->set_text("$[description]");

    {
        double currentVelocity = (startPoint.groundspeed + endPoint.groundspeed)/2;

        // Set the linestyle. The color is a function of speed.
        LineStylePtr lineStyle = factory->CreateLineStyle();
        lineStyle->set_color(mapVelocity2Color(currentVelocity));

        PolyStylePtr polyStyle = factory->CreatePolyStyle();
        polyStyle->set_color(mapVelocity2Color(currentVelocity, 100));

        // Link the style to the icon
        StylePtr style = factory->CreateStyle();
        style->set_balloonstyle(balloonStyle);
        style->set_linestyle(lineStyle);
        style->set_polystyle(polyStyle);

        PairPtr pair = factory->CreatePair();
        pair->set_styleselector(style);
        pair->set_key(kmldom::STYLESTATE_NORMAL);

        styleMap->add_pair(pair);
    }

    {
        double currentVelocity = (startPoint.groundspeed + endPoint.groundspeed)/2;

        // Set the linestyle. The color is a function of speed.
        LineStylePtr lineStyle = factory->CreateLineStyle();
        lineStyle->set_color(mapVelocity2Color(currentVelocity));

        PolyStylePtr polyStyle = factory->CreatePolyStyle();
        polyStyle->set_color(mapVelocity2Color(currentVelocity, 100));
        polyStyle->set_fill(false);

        // Link the style to the icon
        StylePtr style = factory->CreateStyle();
        style->set_balloonstyle(balloonStyle);
        style->set_linestyle(lineStyle);
        style->set_polystyle(polyStyle);

        PairPtr pair = factory->CreatePair();
        pair->set_styleselector(style);
        pair->set_key(kmldom::STYLESTATE_HIGHLIGHT);

        styleMap->add_pair(pair);
    }

    PlacemarkPtr placemark = factory->CreatePlacemark();
    placemark->set_geometry(linestring);
    placemark->set_styleselector(styleMap);
    placemark->set_visibility(true);

    // Create the timespan
    TimeSpanPtr timeSpan = factory->CreateTimeSpan();
    QDateTime startTime = QDateTime::currentDateTimeUtc().addMSecs(newPlacemarkTime); // FIXME: Make this a function of the true time, preferably gotten from the GPS
    QDateTime endTime = QDateTime::currentDateTimeUtc().addMSecs(newPlacemarkTime);
    timeSpan->set_begin(startTime.toString(dateTimeFormat).toStdString());
    timeSpan->set_end(endTime.toString(dateTimeFormat).toStdString());

    // Set the name
    QDateTime trackTime = QDateTime::currentDateTimeUtc().addMSecs(newPlacemarkTime); // FIXME: Make it a function of the realtime preferably gotten from the GPS
    placemark->set_name(trackTime.toString(dateTimeFormat).toStdString());

    // Add a nice description to the track placemark
    placemark->set_description(informationString.toStdString());

    // Set the timespan
    placemark->set_timeprimitive(timeSpan);

    return placemark;
}
コード例 #4
0
ファイル: kmlexport.cpp プロジェクト: 1heinz/TauLabs
/**
 * @brief KmlExport::exportToKML Triggers logfile export to KML.
 */
bool KmlExport::exportToKML()
{
    bool ret = open();
    if (!ret) {
        qDebug () << "Logfile failed to open during KML export";
        return false;
    }

    // Parses logfile and generates KML document
    ret = preparseLogFile();
    if (!ret) {
        qDebug () << "Logfile preparsing failed";
        return false;
    }

    // Call parser.
    parseLogFile();

    // Add track to <Document>
    document->add_feature(trackFolder);

    // Add timespans to <Document>
    document->add_feature(timestampFolder);

    // Add ground track to <Document>
    {
        LineStringPtr linestring = factory->CreateLineString();
        linestring->set_extrude(false); // Do not extrude to ground
        linestring->set_altitudemode(kmldom::ALTITUDEMODE_CLAMPTOGROUND);
        linestring->set_coordinates(wallAxes[0]);

        MultiGeometryPtr multiGeometry = factory->CreateMultiGeometry();
        multiGeometry->add_geometry(linestring);

        PlacemarkPtr placemark = factory->CreatePlacemark();
        placemark->set_geometry(multiGeometry);
        placemark->set_styleurl("#ts_2_tb");
        placemark->set_name("Ground track");

        document->add_feature(placemark);
    }

    // Add wall axes to <Document>
    FolderPtr folder = factory->CreateFolder();
    for (int i=0; i<numberOfWallAxes; i++) {
        LineStringPtr linestring = factory->CreateLineString();
        linestring->set_extrude(false); // Do not extrude to ground
        linestring->set_altitudemode(kmldom::ALTITUDEMODE_ABSOLUTE);
        linestring->set_coordinates(wallAxes[i]);

        MultiGeometryPtr multiGeometry = factory->CreateMultiGeometry();
        multiGeometry->add_geometry(linestring);

        PlacemarkPtr placemark = factory->CreatePlacemark();
        placemark->set_geometry(multiGeometry);
        placemark->set_styleurl("#ts_1_tb");

        folder->add_feature(placemark);
        folder->set_name("Wall axes");
    }
    document->add_feature(folder);

    // Create <kml> and give it <Document>.
    KmlPtr kml = factory->CreateKml();
    kml->set_feature(document);  // kml takes ownership.

    // Serialize to XML
    std::string kml_data = kmldom::SerializePretty(kml);

    // Save to file
    if (QFileInfo(outputFileName).suffix().toLower() == "kmz") {
        if (!kmlengine::KmzFile::WriteKmz(outputFileName.toStdString().c_str(), kml_data)) {
            qDebug() << "KMZ write failed: " << outputFileName;
            QMessageBox::critical(new QWidget(),"KMZ write failed", "Failed to write KMZ file.");
            return false;
        }
    } else if (QFileInfo(outputFileName).suffix().toLower() == "kml") {
        if (!kmlbase::File::WriteStringToFile(kml_data, outputFileName.toStdString())) {
            qDebug() << "KML write failed: " << outputFileName;
            QMessageBox::critical(new QWidget(),"KML write failed", "Failed to write KML file.");
            return false;
        }
    } else {
        qDebug() << "Write failed. Invalid file name:" << outputFileName;
        QMessageBox::critical(new QWidget(),"Write failed", "Failed to write file. Invalid filename");
        return false;
    }


    return true;
}