autopilot/drone_controller/az_mission.cpp

#include <filesystem>
#include <iostream>
#include <string>

#include "../3rd/rapidjson/include/rapidjson/document.h"
#include "../3rd/rapidjson/include/rapidjson/error/en.h"
#include "../3rd/rapidjson/include/rapidjson/filereadstream.h"

#include "az_mission.h"

using namespace rapidjson;
using namespace std;

AzMission::AzMission(string filename)
    : mFilename(filename)
    , mOperationalHeight(INVALID_HEIGHT)
    , mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE))
{
    if (filesystem::exists(mFilename) == false) {
        cerr << "JSON file " << mFilename << " doesn't exists." << endl;
        exit(1);
    }

    parse();
}

void AzMission::parse(void)
{
    FILE *fp = fopen(mFilename.c_str(), "r");
    char readBuffer[65536];
    FileReadStream is(fp, readBuffer, sizeof(readBuffer));

    Document document;
    document.ParseStream(is);
    fclose(fp);

    if (document.HasParseError()) {
        cerr << "JSON parse error: " << GetParseError_En(document.GetParseError()) << endl;
        exit(1);
    }

    // Parse operational height
    int operational_height = document["operational_height"].GetInt();
    if (operational_height > 0) {
        mOperationalHeight = operational_height;
    }

    // Parse geofences
    const Value &geofences = document["geofences"];
    const Value &polygons  = geofences["polygons"];
    for (SizeType i = 0; i < polygons.Size(); i++) {
        AzGeofence geofence;
        const Value &polygon = polygons[i];
        const Value &points  = polygon["points"];
        for (SizeType j = 0; j < points.Size(); j++) {
            AzCoordinate coord(points[j][0].GetDouble(), points[j][1].GetDouble());
            geofence.coordinates.push_back(coord);
        }
        string fence_type = polygon["fence_type"].GetString();
        if (fence_type == "INCLUSION") {
            geofence.type = AZ_GEOFENCE_TYPE_INCLUSION;
        }
        else if (fence_type == "INCLUSION") {
            geofence.type = AZ_GEOFENCE_TYPE_EXCLUSION;
        }

        mGeofences.push_back(geofence);
    }

    // Parse action points
    const Value &action_points = document["action_points"];
    for (SizeType i = 0; i < action_points.Size(); i++) {
        double lat = action_points[i]["point"][0].GetDouble();
        double lon = action_points[i]["point"][1].GetDouble();
        AzCoordinate point(lat, lon);

        int height = action_points[i]["height"].GetInt();

        string action_enum                  = action_points[i]["action_enum"].GetString();
        AzActionPointType action_point_type = AZ_ACTION_POINT_TYPE_NONE;
        if (action_enum == "waypoint") {
            action_point_type = AZ_ACTION_POINT_TYPE_WAYPOINT;
        }
        else if (action_enum == "search") {
            action_point_type = AZ_ACTION_POINT_TYPE_SEARCH;
        }
        else if (action_enum == "return") {
            action_point_type = AZ_ACTION_POINT_TYPE_RETURN;
        }

        uint actionSpefific = 0;
        if (action_points[i].HasMember("action_specific")) {
            const Value &targets = action_points[i]["action_specific"]["targets"];
            for (SizeType j = 0; j < targets.Size(); j++) {
                if (targets[j].GetString() == string("tank")) {
                    actionSpefific |= AZ_ACTION_SPECIFIC_TANK;
                }
                else if (targets[j].GetString() == string("artillery")) {
                    actionSpefific |= AZ_ACTION_SPECIFIC_ARTILLERY;
                }
            }
        }

        AzActionPoint actionPoint(point, height, action_point_type, actionSpefific);
        mActionPoints.push_back(actionPoint);
    }

    // Parse return_point
    mReturnPoint = AzCoordinate(document["return_point"][0].GetDouble(), document["return_point"][1].GetDouble());
}

const vector<AzGeofence> &AzMission::getGeofences(void) const
{
    return mGeofences;
}

const vector<AzActionPoint> &AzMission::getActionPoints(void) const
{
    return mActionPoints;
}

int AzMission::getOperationalHeight(void) const
{
    return mOperationalHeight;
}

AzCoordinate AzMission::getReturnPoint(void) const
{
    return mReturnPoint;
}

ostream &operator<<(ostream &os, const AzMission &obj)
{
    os << "\nMission defination:" << endl;
    os << "----------------------" << endl;
    os << "Filename: " << obj.mFilename << endl;
    os << "Operational height: " << obj.mOperationalHeight << endl;
    os << "Return point: " << obj.mReturnPoint << endl;

    for (size_t i = 0; i < obj.mActionPoints.size(); i++) {
        os << "\nAction point[" << i << "]:" << endl;
        os << obj.mActionPoints[i];
    }
    return os;
}