receiver.cpp

// This file is for demonstrating a Tangram Pro generated "bump-in-the-line"
// transform. In this case the transform is the STANAG-4586 InertialStates to
// LMCP AirVehicleState transform. The transform app is generated by Tangram Pro
// to create the environment around the transform function, which performs the
// message-to-message transformation. The transform app adds transports, 
// serializers and other glue logic to make a complete standalone application.
// The code that follows generates InertialStates messages and sends them to
// the transform app, which should already be running. When the transform app
// receives the message, it converts it to an LMCP AirVehicleState message,
// which can be sent to the OpenAMASE graphical airborne simulation platform.
// OpenAMASE will display the status of a simulated UAV (ID=600) in real-time
// as transformed AirVehicleState messages are received from this application.
// To use this application, a ZeroMQ proxy must also be running. That proxy
// must be listening on TCP port 6667 and publishing on TCP port 6668. The 
// correct startup order is 1) ZeroMQ proxy, 2) is2avs transform app startup,
// 3) start OpenAMASE and select the waterway search example scenario from 
// OpenUxAS and 4) start this application.

// Headers for the messages needed. The first is from STANAG-4586 and the next
// two are from LMCP
#include "InertialStates.hpp"
#include "AirVehicleState.hpp"
#include "AirVehicleConfiguration.hpp"

// We will use a ZeroMQ pub/sub transport. This means we need a ZeroMQ proxy
// running as well. If using the Tangram ZeroMQProxy, the command to start it
// would be:  ./zmq_proxy tcp://*:6667 tcp://*:6668
#include "TangramTransportZMQ.hpp"

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>

// We need an LMCP serializer. This is required to format the transformed
// AirVehicleConfiguration and AirVehicleState messages before sending them to
// OpenAMASE.
#include "LMCPSerializer.h"
#include "afrl_cmasi_DerivedEntityFactory.hpp"
afrl::cmasi::DerivedEntityFactory *lmcpEntityFactory;
tangram::serializers::LMCPSerializer *lmcpSerializer;

// We need a STANAG-4586 serializer. This is required to send the STANAG-4586
// InertialStates message to the Tangram Pro generated transformer app. What
// comes back out is an LMCP AirVehicleState message.
#include "STANAG4586Serializer.h"
#include "tangram_stanag4586_DerivedEntityFactory.hpp"
tangram::stanag4586::DerivedEntityFactory *stanag4586EntityFactory;
tangram::serializers::STANAG4586Serializer *stanag4586Serializer;

// Some constants for the LMCP TCP wrapper. To send LMCP to OpenAMASE via TCP
// stream socket, there's an additional wrapper required around the serialized
// LMCP message.
#define HEADER_SIZE 8
#define CHECKSUM_SIZE 4

// The file descriptor for the TCP socket to OpenAMASE
static int openAMASESocket = -1;

// Forward references for the function that sends an AirVehicleConfiguration
// message to OpenAMASE before we start sending it any transformed
// AirVehicleState messages
static int sendAirVehicleConfiguration();

// Performs an on-demand connect to OpenAMASE via TCP stream socket
static int connectToOpenAMASE() {
    // Talking to OpenAMASE is done using a regular old TCP stream socket
    openAMASESocket = socket(AF_INET, SOCK_STREAM, 0);
    if (openAMASESocket == -1) {
        fprintf(stderr, "Failed to create socket (%d) (%s).\n", openAMASESocket,
            strerror(errno));
        return -1;
    }
    signal(SIGPIPE, SIG_IGN);

    // OpenAMASE sets up a server socket on 0.0.0.0:5555
    struct sockaddr_in addr;
    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(5555);
    inet_pton(AF_INET, "127.0.0.1", &addr.sin_addr.s_addr);
    if (connect(openAMASESocket, (const struct sockaddr *) &addr, sizeof(addr))
        != 0) {
        fprintf(stderr, "Failed to connect socket (%s).\n", strerror(errno));
        close(openAMASESocket);
        openAMASESocket = -1;
        return -1;
    }
    fprintf(stderr, "Connected to OpenAMASE.\n");

    if (sendAirVehicleConfiguration() != 0) {
        return -1;
    }

    return 0;
}

// Performs a blocking write of the given data buffer to the OpenAMASE TCP
// stream socket.
static int sendBytesToOpenAMASE(const unsigned char *data, uint32_t numBytes) {

    // this is a connect-on-demand setup so if the connection to OpenAMASE
    // hasn't already been made, do that now
    if (openAMASESocket == -1) {
        if (connectToOpenAMASE() != 0) {
            return -1;
        }
    }

    // socket send loop, doesn't exit until all bytes given are sent or an
    // error is detected
    uint32_t bytesSent = 0;
    while(bytesSent != numBytes) {
        int status = write(openAMASESocket, &data[bytesSent], numBytes -
            bytesSent);
        if (status <= 0) {
            close(openAMASESocket);
            openAMASESocket = -1;
            return -1;
        } else {
            bytesSent += status;
        }
    }

    return numBytes;
}

// Sends a complete LMCP message to OpenAMASE property wrapping it to be
// compatible with the TCP server setup of OpenAMASE. It will perform the
// initial connection if needed and send the initial AirVehicleConfiguration
// message as well.
static int sendMessageToOpenAMASE(const unsigned char *data, uint32_t messageSize,
    const char *messageName) {
        
    // This code is a fairly direct port of Java code found in the OpenUxAS
    // LMCP Java library

    std::string attributes = messageName;
    attributes += "$lmcp|";
    attributes += messageName;
    attributes += "||0|0$";

    char buf[256];
    snprintf(buf, 256, "%lu", attributes.size() + messageSize + HEADER_SIZE +
        CHECKSUM_SIZE);
    std::string sentinel = "+=+=+=+=";
    sentinel += buf;
    sentinel += "#@#@#@#@";

    std::vector<uint8_t> addressedPayload;
    std::copy(attributes.begin(), attributes.end(),
        std::back_inserter(addressedPayload));
    std::copy(data, data + messageSize, std::back_inserter(addressedPayload));

    uint32_t val = 0;
    for (uint32_t i = 0; i < addressedPayload.size(); i++) {
        val += (addressedPayload[i] & 0xFF);
    }

    std::vector<uint8_t> frontBytes;
    std::copy(sentinel.begin(), sentinel.end(), std::back_inserter(frontBytes));
    std::copy(addressedPayload.begin(), addressedPayload.end(),
        std::back_inserter(frontBytes));

    std::string footer = "!%!%!%!%";
    snprintf(buf, 256, "%u", val);
    footer += buf;
    footer += "?^?^?^?^";

    std::vector<uint8_t> completeMessage;
    std::copy(frontBytes.begin(), frontBytes.end(),
        std::back_inserter(completeMessage));
    std::copy(footer.begin(), footer.end(), std::back_inserter(completeMessage));

    fprintf(stderr, "Sending message of %lu bytes.\n", completeMessage.size());
    int status = sendBytesToOpenAMASE(&completeMessage[0], completeMessage.size());
    if (status > 0) {
        printf("Sent message %s to OpenAMASE\n", messageName);
    } else {
        fprintf(stderr, "Failed to send message %s to OpenAMASE!\n", messageName);
    }
    return status;
}

// Construct and build and AirVehicleConfiguration message suitable for our
// scenario, then send it to OpenAMASE. A new connection will be established
// first if necessary.
static int sendAirVehicleConfiguration() {

    // Note that the AirVehicleConfiguration ID is set to 600. This is what 
    // the new UAV will be seen as in the OpenAMASE application. Also note 
    // that the AirVehicleState messages to follow must have that same ID 
    // value set so that they can be matched up to the configuration message 
    // for that vehicle.
    afrl::cmasi::AirVehicleConfiguration avc;
    avc.setID(600, false);
    avc.setAffiliation("Tangram Flex", false);
    avc.setLabel("Tangram UAV", false);
    avc.setNominalSpeed(25.0, false);
    avc.setNominalAltitude(800.0f, false);
    avc.setNominalAltitudeType("MSL");
    avc.setMinimumSpeed(18.044F, false);
    avc.setMaximumSpeed(33.223f, false);
    avc.setMinimumAltitude(0.0f);
    avc.setMinAltitudeType("AGL", false);
    avc.setMaximumAltitude(100000.0f, false);
    avc.setMaxAltitudeType("MSL", false);
    avc.addToAvailableLoiterTypes(afrl::cmasi::LoiterType::EnumItem::Circular,
        false);
    avc.addToAvailableLoiterTypes(
        afrl::cmasi::LoiterType::EnumItem::FigureEight, false);
    avc.addToAvailableTurnTypes(afrl::cmasi::TurnType::EnumItem::TurnShort,
        false);
    avc.addToAvailableTurnTypes(afrl::cmasi::TurnType::EnumItem::FlyOver,
        false);

    printf("Serializing AirVehicleConfiguration\n");
    std::vector<uint8_t> avcBytes;
    if (!lmcpSerializer->serialize(avc, avcBytes)) {
        fprintf(stderr, "Failed to serialize LMCP message.\n");
        return 1;
    }
    printf("Sending AirVehicleConfiguration (%lu bytes)\n",
        avcBytes.size());
    if (sendMessageToOpenAMASE(&avcBytes[0], avcBytes.size(),
        "afrl.cmasi.AirVehicleConfiguration") == -1) {
        fprintf(stderr, "ERROR SENDING AirVehicleConfiguration (%s)\n",
            strerror(errno));
    }
    printf("Sent AirVehicleConfiguration message.\n");

    return 0;
}

// Initializes the transport. We use ZeroMQ here but this could be something
// else such as ActiveMQ, etc. Just make sure the transformer app is using the
// same incoming and outgoing transport. ZeroMQ is used because that's the
// default for a transformer app so the user doesn't need to change any of the
// defaults.
static int initializeTransport(tangram::transport::TangramTransport *transport,
    std::string subscribeTopic) {

    // Do not use any possible file-supplied options, we will set our own
    tangram::transport::TangramTransport::resetTransportOptions();

    // This can also be done via a configuration file
    transport->setOption("SubscribeIP", "127.0.0.1");
    transport->setOption("SubscribePort", "6668");
    transport->setOption("PollTimeout", "100");
    transport->setOption("PublishIP", "127.0.0.1");
    transport->setOption("PublishPort", "6667");
    transport->setOption("PollTimeout", "100");
    if (transport->open(TTF_READ | TTF_WRITE) != 0) {
        return -1;
    }
    transport->subscribe(subscribeTopic);

    return 0;
}

// Initializes the needed serializers. We have to use an LMCP serializer to send
// messages to OpenAMASE. It should be possible to use other serializers (so
// long as they are compatible with the STANAG-4586 CSI) for sending messages to
// the transformer app. But, it's best to use the native serializer. Make sure
// the transformer app is using the same one for the incoming message.
static int initializeSerializers() {

    // We need an LMCP serializer to send the AirVehicleConfiguration message
    // (no transform for that) and to deserialize and re-serialize the
    // transformed LMCP AirVehicleState messages (to include the platform ID
    // value).
    lmcpEntityFactory = new afrl::cmasi::DerivedEntityFactory();
    lmcpSerializer = new tangram::serializers::LMCPSerializer(lmcpEntityFactory);

    // We need a STANAG-4586 serializer to serialize the InertialStates
    // messages, which are transformed to LMCP AirVehicleState messages.
    stanag4586EntityFactory = new tangram::stanag4586::DerivedEntityFactory();
    stanag4586Serializer = new tangram::serializers::STANAG4586Serializer(
        stanag4586EntityFactory);

    return 0;
}

// Cleans up previously created serializers
static void destroySerializers() {
    delete lmcpSerializer;
    delete lmcpEntityFactory;
    delete stanag4586Serializer;
    delete stanag4586EntityFactory;
}

#define TO_RADS(x) (x/180.0*3.14159)

int main(int argc, char *argv[]) {

    // Setup a hard-coded ZeroMQ path to the bump-on-the-line transformer app
    tangram::transport::TangramTransportZMQ transportZmq;
    if (initializeTransport(&transportZmq, "AirVehicleState") != 0) {
        fprintf(stderr, "Failed to initialize transport.\n");
        return 1;
    }

    // Setup the serializers needed
    initializeSerializers();

    printf("Created and initialized transport and serializers.\n");

    // Pause just a second for the user to see things have initialized
    // successfully
    sleep(1);

    // These are for the values we modify periodically when modifying the
    // InertialStates message
    double longitude = TO_RADS(-121.00538540744407);
    double latitude = TO_RADS(45.30724068719066);
    double altitude = 755.0;
    double heading = TO_RADS(90.0);
    int count = 0;
    double multiplier = 1.0;

    // This is the InertialStates message we will be using, set some static
    // values at this time and update position periodically
    tangram::stanag4586::InertialStates isMsg;
    isMsg.setVehicleID(12345, false);
    isMsg.setCUCSID(22222, false);
    isMsg.setAltitudeType("AGL", false);

    // Every second update the InertialStates message, serialize it and send it
    // to the is2avs transformer app to be converted to an AirVehicleState
    // message
    while (1) {

        // Update the position and heading
        isMsg.setLatitude(latitude, false);
        isMsg.setLongitude(longitude, false);
        isMsg.setAltitude(altitude, false);
        isMsg.setHeading(heading, false);
        isMsg.setTimestamp((double) time(NULL), false);

        // Send the InertialStates message to the transformer app
        std::vector<uint8_t> isMsgBytes;
        if (!stanag4586Serializer->serialize(isMsg, isMsgBytes)) {
            fprintf(stderr, "Failed to serialize STANAG-4586 message.\n");
            break;
        }
        if (transportZmq.publish((const uint8_t *) isMsgBytes.data(),
            (uint32_t) isMsgBytes.size(), "InertialStates", 0) < 0) {
            fprintf(stderr, "Failed to send STANAG-4586 message.\n");
            break;
        }
        printf("Published InertialStates message\n");

        //////////////////////////////////////////////////////////////////////
        // Wait for the transformed AirVehicleState message
        //////////////////////////////////////////////////////////////////////
        uint8_t lmcpBuffer[512];
        int32_t status = transportZmq.recv(lmcpBuffer, 512, 0);
        if (status < 0) {
            fprintf(stderr, "Failed to read incoming AVS message.\n");
            break;
        }
        printf("Received AirVehicleState message\n");

        // We need to deserialize it to set the ID value to match that of the
        // previously sent AirVehicleConfiguration messages.
        // NOTE: This could be eliminated with a transform that properly mapped
        //       an InertialStates field (such as vehicle ID) to the LMCP
        //       AirVehicleState message ID field. Currently that doesn't
        //       happen so we update the transformed message accordingly.
        std::vector<uint8_t> avsMsgBytes(lmcpBuffer, lmcpBuffer + status);
        afrl::cmasi::AirVehicleState avsMsg;
        if (!lmcpSerializer->deserialize(avsMsgBytes, avsMsg)) {
            fprintf(stderr, "Failed to deserialize transformed AVS message.\n");
            break;
        }
        avsMsg.setID(600, false);
        avsMsg.setMode("FlightDirector", false);

        // Re-serialize (again, this could be eliminated with a more thorough
        // mapping of the messages)
        std::vector<uint8_t> lmcpAvsBytes;
        if (!lmcpSerializer->serialize(avsMsg, lmcpAvsBytes)) {
            fprintf(stderr, "Failed to re-serialialize transformed AVS message.\n");
            break;
        }

        // Send the AirVehicleState message to OpenAMASE
        if (sendMessageToOpenAMASE(lmcpAvsBytes.data(), lmcpAvsBytes.size(),
            "afrl.cmasi.AirVehicleState") == -1) {
            fprintf(stderr, "Failed to send message to OpenAMASE.\n");
            break;
        }
        printf("Sent AirVehicleState message to OpenAMASE\n");

        // This is just a debug dump of the final AVS message to the console
        isMsg.dump();
        avsMsg.dump();

        // Sleep for 1s, this is a 1Hz loop
        sleep(1);

        // Bump the latitude, altitude and energy; latitude depends on where
        // we are, it may go up or down
        longitude += TO_RADS(0.0021223 * multiplier);
        altitude += 1.2;

        // Every 30 updates, swap the direction of travel
        count++;
        if (count == 30) {
            count = 0;
            if (multiplier < 0.0) {
                heading = TO_RADS(90.0);
            } else {
                heading = TO_RADS(270.0);
            }
            multiplier *= -1.0;
        }
    }

    // Cleanup before leaving

    if (openAMASESocket != -1) {
        close(openAMASESocket);
    }
    transportZmq.close();
    destroySerializers();

    return 0;
}