ZeroMQ/Protobuf Test

Makefile

.PHONY: clean

all: receiver sender SensorStates

SensorStates: SensorStates.proto
        protoc --cpp_out=. SensorStates.proto

sender: sender.cpp SensorStates
        g++ -o sender sender.cpp SensorStates.pb.cc -lzmq -lprotobuf

receiver: receiver.cpp SensorStates
        g++ -o receiver receiver.cpp SensorStates.pb.cc -lzmq -lprotobuf

clean:
        rm -f *.o sender receiver SensorStates.pb.cc SensorStates.pb.h

receiver.cpp

#include <zmq.hpp>
#include <string>
#include <iostream>
#include <unistd.h>
#include <cstdio>
#include "SensorStates.pb.h"
#include <time.h>

int main() {
  // Prepare our context and socket
  zmq::context_t context(1);
  zmq::socket_t socket(context, ZMQ_PAIR);
  socket.bind("tcp://*:5555");

  int recv_count = 0;
  const int window_size = 100000;
  const double max_time = 5.0; // seconds
  struct timespec total_time;
  total_time.tv_sec = total_time.tv_nsec = 0;

  struct timespec *last_time = new struct timespec();
  struct timespec *curr_time = new struct timespec();
  if(clock_gettime(CLOCK_MONOTONIC, last_time) < 0)
    {
      perror("Error initializing clock\n");
      delete last_time;
      delete curr_time;
      exit(1);
    }
  while(true) {
    zmq::message_t request(100);

    // Wait for next request from client
    socket.recv(&request);
    recv_count++;
    //printf("recv_count: %d\n", recv_count);

    // get the current time
    if(clock_gettime(CLOCK_MONOTONIC, curr_time) < 0)
      {
        perror("Error getting current time\n");
        delete last_time;
        delete curr_time;
        exit(1);
      }

    //printf("curr_time: %lds %ldns\n", (long)curr_time->tv_sec, curr_time->tv_nsec);
    //printf("last_time: %lds %ldns\n", (long)last_time->tv_sec, last_time->tv_nsec);

    // calculate the new time totals
    total_time.tv_sec += curr_time->tv_sec - last_time->tv_sec;
        total_time.tv_nsec += curr_time->tv_nsec - last_time->tv_nsec;
    while(total_time.tv_nsec >= 1000000000) // rollover
      {
        total_time.tv_sec += 1;
        total_time.tv_nsec = total_time.tv_nsec - 1000000000;
      }

    // swap curr and last time pointers
    struct timespec* temp;
    temp = last_time;
    last_time = curr_time;
    curr_time = temp;

    // see if the sampling time is over
    double time = (double)total_time.tv_sec + (double)total_time.tv_nsec/1000000000.0;

    //printf("total_time: %lds %ldns\n", (long)total_time.tv_sec, total_time.tv_nsec);
    //printf("time: %3.3f\n", time);
    if(time >= max_time || (recv_count >= window_size && time > 0.0))
      {
        printf("Rate: %3.3fHz with window %d and total elapsed time %3.3f\n", (double)recv_count/time, recv_count, time);

        total_time.tv_sec = 0;
        total_time.tv_nsec = 0;
        recv_count = 0;
      }

    //SensorStates states;
    // states.ParseFromArray(request.data(), request.size());
    /*std::cout << "Received States" << std::endl;
    printf("\twheel angle: %3.3f\n", states.wheel_angle());
    printf("\tgas: %3.1f\n", states.gas());
    printf("\tbrake: %3.1f\n", states.brake());
    printf("\tvibration: %3.1f\n", states.vibration());*/


    //    if(recv_count % 1000 == 0)
    //  printf("Received another 1000 messages\n");
  }

  delete last_time;
  delete curr_time;

  return 0;
}

sender.cpp

#include <zmq.hpp>
#include <string>
#include <iostream>
#include "SensorStates.pb.h"

int main()
{
  zmq::context_t context(1);
  zmq::socket_t socket(context, ZMQ_PAIR);

  std::cout << "Connecting to hello world server..." << std::endl;
  socket.connect("tcp://localhost:5555");

  std::string states_str;
  SensorStates states;
  int sz;
  zmq::message_t* request;


  while(true) {

    states.set_wheel_angle(3.14159);
    states.set_gas(1.0);
    states.set_brake(1.0);
    states.set_vibration(0.5);
    
    states.SerializeToString(&states_str);

    sz = states_str.length();

    request = new zmq::message_t(sz);
    memcpy((void *)request->data(), states_str.c_str(), sz);
    socket.send(*request);

    delete request;
    usleep(1000); // not perfect timing, but should be good enough
  }

  return 0;
}

SensorStates.proto

message SensorStates {
        required float wheel_angle = 1;
        required float gas = 2;
        required float brake = 3;
        required float vibration = 4;
}