vrpn_Tracker_JsonNet.C

Go to the documentation of this file.

#include "vrpn_Tracker_JsonNet.h"
 
#if defined(VRPN_USE_JSONNET)
 
#ifdef _WIN32
  #ifdef VRPN_USE_WINSOCK2
    #include <winsock2.h>    // struct timeval is defined here
  #else
    #include <winsock.h>    // struct timeval is defined here
  #endif
#else
    #include <sys/socket.h>
    #include <sys/time.h>
    #include <netinet/in.h>
    #include <unistd.h>
    #define INVALID_SOCKET -1
#endif
 
#include "json/json.h"
 
#include "quat.h"
 
#include "vrpn_SendTextMessageStreamProxy.h"
 
#include <stdlib.h> // for exit
 
// These must match definitions in eu.ensam.ii.vrpn.Vrpn
static const char* const MSG_KEY_TYPE =             "type";
//static const char* const MSG_KEY_SEQUENCE_NUMBER =    "sn";
//static const char* const MSG_KEY_TIMESTAMP =      "ts";
    
static const char* const MSG_KEY_TRACKER_ID =       "id";
static const char* const MSG_KEY_TRACKER_QUAT =     "quat";
static const char* const MSG_KEY_TRACKER_POS =      "pos";
 
static const char* const MSG_KEY_BUTTON_ID =        "button";
static const char* const MSG_KEY_BUTTON_STATUS =    "state";
    
static const char* const MSG_KEY_ANALOG_CHANNEL =   "num";
static const char* const MSG_KEY_ANALOG_DATA =      "data";
 
static const char* const MSG_KEY_TEXT_DATA =        "data";
 
// Message types (values for MSG_KEY_TYPE)
static const int MSG_TYPE_TRACKER = 1;
static const int MSG_TYPE_BUTTON = 2;
static const int MSG_TYPE_ANALOG = 3;
static const int MSG_TYPE_TEXT = 4;
 
vrpn_Tracker_JsonNet::vrpn_Tracker_JsonNet(const char* name,vrpn_Connection* c,int udp_port) :
    vrpn_Tracker(name, c),
    vrpn_Button_Filter(name, c),
    vrpn_Analog(name, c),
    vrpn_Text_Sender(name, c),
    _socket(INVALID_SOCKET),
    _do_tracker_report(false),
    _pJsonReader(0)
{
    fprintf(stderr, "vrpn_Tracker_JsonNet : Device %s listen on port udp port %d\n", name, udp_port);
    if (! _network_init(udp_port)) {
        exit(EXIT_FAILURE);
    }
 
    // Buttons part
 
    num_buttons = vrpn_BUTTON_MAX_BUTTONS;
    num_channel = vrpn_CHANNEL_MAX;
 
    _pJsonReader = new Json::Reader();
}
 
vrpn_Tracker_JsonNet::~vrpn_Tracker_JsonNet(void)
{
    if (_pJsonReader != 0) {
                try {
                  delete _pJsonReader;
                } catch (...) {
                  fprintf(stderr, "vrpn_Tracker_JsonNet::~vrpn_Tracker_JsonNet(): delete failed\n");
                  return;
                }
        _pJsonReader = 0;
    }
    _network_release();
}
 
 
void vrpn_Tracker_JsonNet::mainloop()
{
    server_mainloop();
    /*
     * The original Dtrack code uses blocking call to select() in _network_receive with 
     * a 1 sec timeout. In Dtrack, the data is supposed to be continuously flowing (app. 60 Hz), 
     * so the timeout is unlikely to happen. However, the data from the Android device flow at a lower
     * frequency and may not flow at all if the tilt tracker is disabled. 
     * Thus a 1 sec timeout here causes latency and jerky movements in Dtrack 
     */
    const int timeout_us = 10 * 1000;
    int received_length = _network_receive(_network_buffer, _NETWORK_BUFFER_SIZE, timeout_us);
 
    if (received_length < 0) {
        //fprintf(stderr, "vrpn_Tracker_JsonNet : receive error %d\n", received_length);
        return;
    } 
    _network_buffer[received_length] = '\0';
    //fprintf(stderr, "got data : %.*s\n", received_length, _network_buffer);
    if (!_parse(_network_buffer, received_length)) {
        // whatever error
 
        return;
    }
 
    // report trackerchanges
    // TODO really use timestamps
    struct timeval ts ;
    vrpn_gettimeofday(&ts, NULL);
    // from vrpn_Tracker_DTrack::dtrack2vrpnbody
    if (d_connection && _do_tracker_report) {
        char msgbuf[1000];
        // Encode pos and d_quat
        int len = vrpn_Tracker::encode_to(msgbuf);
        if (d_connection->pack_message(len, ts, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY)) {
            // error
                    return;
                }
        _do_tracker_report = false;
        //fprintf(stderr, "Packed and sent\n");
    }
 
    vrpn_Button::report_changes();
    vrpn_Analog::report_changes();
}
 
bool vrpn_Tracker_JsonNet::_parse(const char* buffer, int /*length*/)
{
    Json::Value root;                           // will contains the root value after parsing.
    // Beware collectcomment = true crashes
    bool parsingSuccessful = _pJsonReader->parse( buffer, root , false);
    if ( !parsingSuccessful ) {
        // report to the user the failure and their locations in the document.
        fprintf(stderr, "vrpn_Tracker_JsonNet parse error :%s\n",
                         _pJsonReader->getFormatedErrorMessages().c_str());
        fprintf(stderr, "%s\n",buffer);
        return false;
    }
 
    const Json::Value& constRoot = root;
    // Find MessageType
    const Json::Value& type = constRoot[MSG_KEY_TYPE]; 
    int messageType;
    if (!type.empty() && type.isConvertibleTo(Json::intValue)) {
        messageType = type.asInt();
        // HACK
    } else {
        fprintf(stderr, "vrpn_Tracker_JsonNet parse error : missing message type\n");
        return false;
    }
    switch (messageType) {
        // TODO cleanup
        case MSG_TYPE_TRACKER:
            return _parse_tracker_data(root);
            break;
        case MSG_TYPE_BUTTON:
            return _parse_button(root);
            break;
        case MSG_TYPE_ANALOG:
            return _parse_analog(root);
            break;
        case MSG_TYPE_TEXT:
            return _parse_text(root);
            break;
        default:
            break;
    }
    return false;
}

bool vrpn_Tracker_JsonNet::_parse_tracker_data(const Json::Value& root)
{
    const Json::Value& constRoot = root;
 
    // Id of the current tracker
    const Json::Value& sensorId = constRoot[MSG_KEY_TRACKER_ID];
    if (!sensorId.empty() && sensorId.isConvertibleTo(Json::intValue)){
        this->d_sensor = sensorId.asInt();
    } else {
        return false;
    }
 
    /*
     * mainloop calls vrpn_Tracker::encode_to, that will send d_sensor, d_pos and d_quat.
     * velocity and acceleration are curretly not handled
     */
     
    const Json::Value& quatData = constRoot[MSG_KEY_TRACKER_QUAT];
    if (!quatData.empty() && quatData.isArray() && quatData.size() == 4) {
        this->d_quat[0] = quatData[0u].asDouble();
        this->d_quat[1] = quatData[1].asDouble();
        this->d_quat[2] = quatData[2].asDouble();
        this->d_quat[3] = quatData[3].asDouble();
        //q_vec_type ypr;
        //q_to_euler(ypr, d_quat);
        //fprintf(stderr, "yaw-rY %.3f pitch-rX %.3f roll-rZ %.3f \n", ypr[0], ypr[1], ypr[2]);
    } 
 
    /*
     * Look for a position 
     */
    const Json::Value& posData = constRoot[MSG_KEY_TRACKER_POS];
    if (!posData.empty() && posData.isArray() &&  posData.size() == 3) {
        this->pos[0] = posData[0u].asDouble();
        this->pos[1]= posData[1].asDouble();
        this->pos[2]= posData[2].asDouble();
    } 
 
    _do_tracker_report = true;
    return true;
}

bool vrpn_Tracker_JsonNet::_parse_text(const Json::Value& root)
{
    const Json::Value& valueTextStatus = root[MSG_KEY_TEXT_DATA];
    if (!valueTextStatus.empty() && valueTextStatus.isConvertibleTo(Json::stringValue)) {
        send_text_message(vrpn_TEXT_NORMAL) << valueTextStatus.asString();
        return true;
    }
    fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_text parse error : missing text");
    return false;
}
bool vrpn_Tracker_JsonNet::_parse_button(const Json::Value& root)
{
    const Json::Value& valueButtonStatus = root[MSG_KEY_BUTTON_STATUS]; 
    bool buttonStatus;
    if (!valueButtonStatus.empty() && valueButtonStatus.isConvertibleTo(Json::booleanValue)) {
        buttonStatus = valueButtonStatus.asBool();
    } else {
        fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_button parse error : missing status");
        return false;
    }
    const Json::Value& valueButtonId = root[MSG_KEY_BUTTON_ID]; 
    int buttonId;       // buttonId embedded in the message. 
    if (!valueButtonId.empty() && valueButtonId.isConvertibleTo(Json::intValue)) {
        buttonId = valueButtonId.asInt();
    } else {
        fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_button parse error : missing id\n");
        return false;
    }
 
    if (buttonId < 0 || buttonId > num_buttons) {
        fprintf(stderr, "invalid button Id %d (max : %d)\n", buttonId, num_buttons);
    } else {
        buttons[buttonId] = (int)buttonStatus;
    }
 
    return true;
}

 
bool vrpn_Tracker_JsonNet::_parse_analog(const Json::Value& root)
{
    const Json::Value& valueData = root[MSG_KEY_ANALOG_DATA]; 
    double data;
    if (!valueData.empty() && valueData.isConvertibleTo(Json::realValue)) {
        data = valueData.asDouble();
    } else {
        fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog parse error : missing status");
        return false;
    }
 
    const Json::Value& channelNumberId = root[MSG_KEY_ANALOG_CHANNEL]; 
    int channelNumber;
    if (!channelNumberId.empty() && channelNumberId.isConvertibleTo(Json::intValue)) {
        channelNumber = channelNumberId.asInt();
    } else {
        fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog parse error : missing id\n");
        return false;
    }
    
    if (channelNumber < 0 || channelNumber >= num_channel) {
        fprintf(stderr, "vrpn_Tracker_JsonNet::_parse_analog id out of bounds %d/%d\n", channelNumber, num_channel);
    } else {
        channel[channelNumber] = data;
    }
 
    return true;
}

bool vrpn_Tracker_JsonNet::_network_init(int udp_port)
{
    int iResult;
#ifdef _WIN32
    {
        // Initialize Winsock
        WORD versionRequested =  MAKEWORD(2,2);
        WSADATA wsaData;
 
        iResult = WSAStartup(versionRequested, &wsaData);
        if (iResult != 0) {
            printf("WSAStartup failed with error: %d\n", iResult);
            return false;
        }
    }
#endif
 
#ifdef _WIN32
    {
        // Create a vrpn_SOCKET for connecting to server
        _socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
        if (_socket == INVALID_SOCKET) {
            printf("socket failed with error: %ld\n", WSAGetLastError());
            //freeaddrinfo(result);
            WSACleanup();
            return false;
        }
    }
#else
    {
        int usock;
        
        usock = socket(PF_INET, SOCK_DGRAM, 0);
    
        if (usock < 0){
            return false;
        }
        _socket = usock;
    }
#endif
    struct sockaddr_in localSocketAddress;
        memset((void *)&localSocketAddress, 0, sizeof(localSocketAddress));
    localSocketAddress.sin_family = AF_INET;
    localSocketAddress.sin_addr.s_addr = htonl(INADDR_ANY);
    localSocketAddress.sin_port = htons(udp_port);
 
    // Setup the listening socket
    iResult = bind( _socket, (struct sockaddr*)&localSocketAddress, sizeof(localSocketAddress));
    if (iResult < 0) {
#ifdef _WIN32
        printf("bind failed with error: %d\n", WSAGetLastError());
#else
        printf("bind failed.");
#endif
        //freeaddrinfo(result);
        _network_release();
        return false;
    }
 
    //freeaddrinfo(result);
    return true;
 
}
 
int vrpn_Tracker_JsonNet::_network_receive(void *buffer, int maxlen, int tout_us)
{
    int nbytes, err;
    fd_set set;
    struct timeval tout;
 
    // waiting for data:
 
    FD_ZERO(&set);
    FD_SET(_socket, &set);
 
    tout.tv_sec = tout_us / 1000000;
    tout.tv_usec = tout_us % 1000000;
 
    switch((err = select(FD_SETSIZE, &set, NULL, NULL, &tout))){
        case 1:
            break;        // data available
        case 0:
            //fprintf(stderr, "net_receive: select timeout (err = 0)\n");
            return -1;    // timeout
            break;
        default:
            //fprintf(stderr, "net_receive: select error %d\n", err);
            return -2;    // error
 
    }
 
    // receiving packet:
    while(1){
 
        // receive one packet:
        nbytes = recv(_socket, (char *)buffer, maxlen, 0);
        if(nbytes < 0){  // receive error
            //fprintf(stderr, "recv_receive: select error %d\n", err);
            return -3;
        }
 
        // check, if more data available: if so, receive another packet
        FD_ZERO(&set);
        FD_SET(_socket, &set);
 
        tout.tv_sec = 0;   // timeout with value of zero, thus no waiting
        tout.tv_usec = 0;
 
        if(select(FD_SETSIZE, &set, NULL, NULL, &tout) != 1){
            // no more data available: check length of received packet and return
            if(nbytes >= maxlen){   // buffer overflow
                return -4;
           }
        return nbytes;
        }
    }
}

void vrpn_Tracker_JsonNet::_network_release()
{
#ifdef _WIN32
    closesocket(_socket);
    WSACleanup();
#else
    close(_socket);
#endif
}
 
#endif // defined VRPN_USE_JSONNET