void send(EventHandler *p, const char *data, int len, bool isDataEnd) {
if (!p) {
INFO_OUT("Invalid context");
return;
}
if (::send((int) (long) p->getContext(), data, len, 0) == -1) {
perror("send");
}
INFO_OUT("Done sending");
}
void send(EventHandler *p, const char *data, int len, bool isDataEnd) {
if (!p) {
INFO_OUT("Invalid context");
return;
}
this->pbuff->len = len;
memcpy(this->pbuff->buffer, data, len);
this->pbuff->destId = (uint16_t)(long)(void*)p->getContext();
sem_t* semDest = (p == this->server) ? semClient : semServer;
if (sem_post(semDest) == -1) {
diep("sem_post");
}
if (semDest == semServer) {
if (this->server) {
semNext = semServer;
}
else {
semNext = semClient;
}
} else {
if (this->client) {
semNext = semClient;
} else {
semNext = semServer;
}
}
}
void intr(int signum)
{
INFO_OUT("Exiting due to signal %d (%s)\n", signum, strsignal(signum));
sigdata->done = 1;
signal(signum, exit);
}
/**
* Create a socket and use ZeroMQ to poll.
*/
void listener()
{
WSADATA wsaData;
int nResult = 0;
int nOptOffVal = 0;
int nOptOnVal = 1;
int nOptLen = sizeof(int);
// Initialize Winsock
nResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (nResult != NO_ERROR)
{
ERROR_OUT("zmqListen : WSAStartup failed");
}
// Create UDP socket
SOCKET fdSocket;
fdSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (fdSocket == INVALID_SOCKET)
{
ERROR_OUT("zmqListen : Socket creation failed");
}
// Set up the sockaddr structure
struct sockaddr_in saListen = {0};
saListen.sin_family = AF_INET;
saListen.sin_port = htons(PING_PORT_NUMBER);
saListen.sin_addr.s_addr = htonl(INADDR_ANY);
// Bind the socket
nResult = bind(fdSocket, (sockaddr*)&saListen, sizeof(saListen));
if (nResult != NO_ERROR)
{
ERROR_OUT("zmqListen : socket bind failed");
}
while (!g_threadInterupted)
{
// Poll socket for a message
zmq::pollitem_t items[] = {{NULL, fdSocket, ZMQ_POLLIN, 0}};
zmq::poll(&items[0], 1, SOCKET_POLL_TIMEOUT);
// If we get a message, print the contents
if (items[0].revents & ZMQ_POLLIN)
{
char recvBuf[PING_MSG_SIZE] = {0};
int saSize = sizeof(struct sockaddr_in);
size_t size = recvfrom(fdSocket, recvBuf, PING_MSG_SIZE + 1, 0, (sockaddr*)&saListen, &saSize);
{
std::string ip(inet_ntoa(saListen.sin_addr));
INFO_OUT("received: " + std::string(recvBuf) + " from " + ip);
}
}
}
closesocket(fdSocket);
WSACleanup();
}
void bindServer(const char *port, EventHandler *pProcessor) {
struct sockaddr_in sin = { 0 };
sin.sin_family = AF_INET;
sin.sin_addr.s_addr = 0;
sin.sin_port = htons(atoi(port));
listener = socket(AF_INET, SOCK_STREAM, 0);
this->server = pProcessor;
setParent(pProcessor);
fcntl(listener, F_SETFL, O_NONBLOCK);
int oneval = 1;
setsockopt(listener, SOL_SOCKET, SO_REUSEADDR, &oneval, sizeof(oneval));
if (bind(listener, (struct sockaddr*) &sin, sizeof(sin)) < 0) {
perror("bind");
return;
}
INFO_OUT("Bound to port %s", port);
if (listen(listener, 16) < 0) {
perror("listen");
return;
}
INFO_OUT("Listenning to port %s", port);
}
void LibEventMain::errorfn(bufferevent *bev, short int error, void *arg) {
INFO_OUT("Errorfn: %x\n", error);
if (error & BEV_EVENT_CONNECTED) {
bufferevent_setwatermark(bev, EV_READ, 0, max_buff);
bufferevent_enable(bev, EV_READ | EV_WRITE);
EventHandler *p = (EventHandler *) arg;
if (p) {
p->enable();
}
}
// if error & BEV_EVENT_EOF, BEV_EVENT_ERROR, BEV_EVENT_TIMEOUT
if ((error & BEV_EVENT_ERROR) || (error & BEV_EVENT_EOF)
|| (error & BEV_EVENT_TIMEOUT)) {
bufferevent_free(bev);
}
}
void WindowInputSource::keyboardInput(const RAWKEYBOARD& keyboard)
{
switch (keyboard.Message)
{
case WM_KEYDOWN:
notifyKeyDown(key_type(keyboard.VKey));
break;
case WM_KEYUP:
notifyKeyUp(key_type(keyboard.VKey));
break;
default:
INFO_OUT(TAG,
"Unrecognized keyboard message: VKey = 0x%X Msg = 0x%X",
keyboard.VKey,
keyboard.Message);
break;
}
}
void LibEventMain::readfn(bufferevent *bev, void *arg) {
EventHandler *p = (EventHandler *) arg;
INFO_OUT("Readfn %s:\n", (p ? p->getDescription() : "None"));
evbuffer *input, *output;
size_t n;
int i;
input = bufferevent_get_input(bev);
char buffer[max_buff];
//
// char *data = evbuffer_readln(input, &n, EVBUFFER_EOL_LF);
// p->process(data, n, true);
// free(data);
if ((n = evbuffer_remove(input, buffer, sizeof(buffer))) > 0) {
p->process(buffer, n, !n);
}
// Todo: What happens to the remaining buffer that is less than max_buff
}
void bindServer(const char *port, EventHandler *pProcessor) {
sockaddr_in sin = { 0 };
sin.sin_family = AF_INET;
sin.sin_port = htons(atoi(port));
int listenerfd = socket(AF_INET, SOCK_STREAM, 0);
if (listenerfd == -1) {
ERRNO_OUT("Error creating listening socket");
exit(1);
}
int reuse = 1;
if (setsockopt(listenerfd, SOL_SOCKET, SO_REUSEADDR, &reuse,
sizeof(reuse))) {
ERRNO_OUT("Error enabling socket reuse");
exit(1);
}
evutil_make_socket_nonblocking(listenerfd);
if (bind(listenerfd, (sockaddr*) &sin, sizeof(sin)) < 0) {
perror("bind");
}
if (listen(listenerfd, 16) < 0) {
perror("listen");
return;
}
setParent(pProcessor);
event *e = event_new(m_ebase, listenerfd, EV_READ | EV_PERSIST,
acceptfn, (void*) pProcessor);
event_add(e, NULL);
INFO_OUT("Bound to port:%s\n", port);
}
void LibEventMain::acceptfn(int listener, short event, void *arg) {
EventHandler *processor = (EventHandler*) arg;
LibEventMain *plevent = (LibEventMain*) processor->getParent();
sockaddr_storage ss;
socklen_t slen = sizeof(ss);
// Investigate: Using a loop to accept connections makes it faster?
int fd = accept(listener, (sockaddr*) &ss, &slen);
if (fd < 0) {
// Investigate: Should check EWOULDBLOCK and EAGAIN?
perror("accept");
return;
}
INFO_OUT("Client connected on fd %d\n", fd);
bufferevent *bev;
evutil_make_socket_nonblocking(fd);
bev = bufferevent_socket_new(plevent->m_ebase, fd, BEV_OPT_CLOSE_ON_FREE);
processor->setContext((Context*) bev);
bufferevent_setcb(bev, readfn, NULL, errorfn, arg);
bufferevent_setwatermark(bev, EV_READ, 0, max_buff);
bufferevent_enable(bev, EV_READ | EV_WRITE);
}
void process() {
int efd;
epoll_event event = {0};
epoll_event events[MAXEVENTS];
efd = epoll_create1(0);
if (efd == -1) {
perror("epoll_create");
exit(1);
}
if (this->listener != -1) {
MyEventData data = {listener, NULL};
event.data.ptr = new MyEventData(data);
event.events = EPOLLIN;
if (epoll_ctl(efd, EPOLL_CTL_ADD, listener, &event) == -1) {
perror("epoll_ctl");
exit(1);
}
}
if (this->dest != -1) {
MyEventData data = {dest, client};
event.data.ptr = new MyEventData(data);
event.events = EPOLLIN;
if (epoll_ctl(efd, EPOLL_CTL_ADD, dest, &event) == -1) {
perror("epoll_ctl");
exit(1);
}
}
while (!loopEnd) {
int nevents = epoll_wait(efd, events, MAXEVENTS, -1);
for (int i=0; i < nevents; i++) {
epoll_event *pev = &events[i];
MyEventData *data = (MyEventData*)pev->data.ptr;
if ((pev->events & EPOLLERR) ||
(pev->events & EPOLLHUP) ||
!(pev->events & EPOLLIN)) {
fprintf(stderr, "epoll error\n");
close(data->fd);
delete data;
continue;
}
if (listener == data->fd) {
struct sockaddr_storage ss;
socklen_t slen = sizeof(ss);
int acceptfd = accept(listener, (struct sockaddr*) &ss, &slen);
if (acceptfd == -1) {
perror("accept");
continue;
}
fcntl(acceptfd, F_SETFL, O_NONBLOCK);
MyEventData adata = {acceptfd, server};
event.data.ptr = new MyEventData(adata);
event.events = EPOLLIN;
epoll_ctl(efd, EPOLL_CTL_ADD, acceptfd, &event);
continue;
}
INFO_OUT("Reading socket %d", i);
char buf[1024];
bool isDone = false;
ssize_t result = recv(data->fd, buf, sizeof(buf), 0);
if (result < 0) {
perror("recv");
close(data->fd);
delete data;
continue;
} else if (result == 0) {
close(data->fd);
delete data;
break;
}
if (data->pHandler) {
data->pHandler->setContext((Context*) (long) data->fd);
data->pHandler->process(buf, result, true);
}
}
}
close(listener);
}
int main(int argc, char *argv[])
{
struct kingrid_info data;
freenect_context *kn;
freenect_device *kn_dev;
int rows = 40, cols = 96; // terminal size
int opt;
sigdata = &data;
data.out_of_range = 0;
data.done = 0;
data.divisions = 6;
data.boxwidth = 10;
data.histrows = 8;
data.frame = 0;
data.zmin = 0.5;
data.zmax = 5.0;
data.disp_mode = STATS;
if(getenv("LINES")) {
rows = atoi(getenv("LINES"));
}
if(getenv("COLUMNS")) {
cols = atoi(getenv("COLUMNS"));
}
// Handle command-line options
while((opt = getopt(argc, argv, "shag:z:Z:")) != -1) {
switch(opt) {
case 's':
// Stats mode
data.disp_mode = STATS;
break;
case 'h':
// Histogram mode
data.disp_mode = HISTOGRAM;
break;
case 'a':
// ASCII art mode
data.disp_mode = ASCII;
break;
case 'g':
// Grid divisions
data.divisions = atoi(optarg);
break;
case 'z':
// Near clipping
data.zmin = atof(optarg);
break;
case 'Z':
// Far clipping
data.zmax = atof(optarg);
break;
default:
fprintf(stderr, "Usage: %s -[sha] [-g divisions] [-zZ distance]\n", argv[0]);
fprintf(stderr, "Use up to one of:\n");
fprintf(stderr, "\ts - Stats mode (default)\n");
fprintf(stderr, "\th - Histogram mode\n");
fprintf(stderr, "\ta - ASCII art mode\n");
fprintf(stderr, "Use any of:\n");
fprintf(stderr, "\tg - Set grid divisions for both dimensions\n");
fprintf(stderr, "\tz - Set near clipping plane in meters for ASCII art mode (default 0.5)\n");
fprintf(stderr, "\tZ - Set far clipping plane in meters for ASCII art mode (default 5.0)\n");
return -1;
}
}
data.boxwidth = (cols - 1) / data.divisions - 3;
if(data.boxwidth < 10) {
data.boxwidth = 10;
}
data.histrows = (rows - 2) / data.divisions - 1;
init_lut(data.depth_lut);
if(signal(SIGINT, intr) == SIG_ERR ||
signal(SIGTERM, intr) == SIG_ERR) {
ERROR_OUT("Error setting signal handlers\n");
return -1;
}
if(freenect_init(&kn, NULL) < 0) {
ERROR_OUT("libfreenect init failed.\n");
return -1;
}
INFO_OUT("Found %d Kinect devices.\n", freenect_num_devices(kn));
if(freenect_num_devices(kn) == 0) {
ERROR_OUT("No Kinect devices present.\n");
return -1;
}
if(freenect_open_device(kn, &kn_dev, 0)) {
ERROR_OUT("Error opening Kinect #0.\n");
return -1;
}
freenect_set_user(kn_dev, &data);
freenect_set_tilt_degs(kn_dev, -5);
freenect_set_led(kn_dev, LED_GREEN);
freenect_set_depth_callback(kn_dev, depth);
freenect_set_depth_format(kn_dev, FREENECT_DEPTH_11BIT);
freenect_start_depth(kn_dev);
int last_oor = data.out_of_range;
while(!data.done && freenect_process_events(kn) >= 0) {
if(last_oor != data.out_of_range) {
freenect_set_led(kn_dev, data.out_of_range ? LED_BLINK_RED_YELLOW : LED_GREEN);
last_oor = data.out_of_range;
}
}
freenect_stop_depth(kn_dev);
freenect_set_led(kn_dev, LED_OFF);
freenect_close_device(kn_dev);
freenect_shutdown(kn);
return 0;
}
void depth(freenect_device *kn_dev, void *depthbuf, uint32_t timestamp)
{
struct kingrid_info *data = freenect_get_user(kn_dev);
uint16_t *buf = (uint16_t *)depthbuf;
int small_histogram[data->divisions][data->divisions][SM_HIST_SIZE];
int total[data->divisions][data->divisions];
uint16_t min[data->divisions][data->divisions];
uint16_t max[data->divisions][data->divisions];
uint16_t median[data->divisions][data->divisions];
float avg[data->divisions][data->divisions];
int oor_count[data->divisions][data->divisions];
int div_pix[data->divisions][data->divisions];
int oor_total = 0; // Out of range count
int i, j, medcount, histcount;
// Initialize data structures
memset(small_histogram, 0, sizeof(small_histogram));
memset(total, 0, sizeof(total));
memset(min, 0xff, sizeof(min));
memset(max, 0, sizeof(max));
memset(oor_count, 0, sizeof(oor_count));
memset(div_pix, 0, sizeof(oor_count));
// Fill in grid stats
for(i = 0; i < FREENECT_FRAME_PIX; i++) {
int gridx = PX_TO_GRIDX(i);
int gridy = PX_TO_GRIDY(i);
div_pix[gridy][gridx]++; // TODO: Calculate this only once
if(buf[i] == 2047) {
oor_count[gridy][gridx]++;
oor_total++;
continue;
}
small_histogram[gridy][gridx][buf[i] * SM_HIST_SIZE / 1024]++;
if(buf[i] < min[gridy][gridx]) {
min[gridy][gridx] = buf[i];
}
if(buf[i] > max[gridy][gridx]) {
max[gridy][gridx] = buf[i];
}
total[gridy][gridx] += buf[i];
}
// Calculate grid averages
for(i = 0; i < data->divisions; i++) {
for(j = 0; j < data->divisions; j++) {
if(oor_count[i][j] < div_pix[i][j]) {
avg[i][j] = (double)total[i][j] / (double)(div_pix[i][j] - oor_count[i][j]);
// FIXME: Something is wrong with median calculation
for(medcount = 0, histcount = 0; histcount < SM_HIST_SIZE; histcount++) {
medcount += small_histogram[i][j][histcount];
if(medcount >= (div_pix[i][j] - oor_count[i][j]) / 2) {
break;
}
}
median[i][j] = (histcount * 1024 + (SM_HIST_SIZE / 2)) / SM_HIST_SIZE;
} else {
min[i][j] = 2047;
max[i][j] = 2047;
avg[i][j] = 2047;
median[i][j] = 2047;
}
}
}
// Display grid stats
printf("\e[H\e[2J");
INFO_OUT("time: %u frame: %d out: %d%%\n", timestamp, data->frame, oor_total * 100 / FREENECT_FRAME_PIX);
for(i = 0; i < data->divisions; i++) {
if(data->disp_mode != ASCII) {
grid_hline(data);
}
switch(data->disp_mode) {
case STATS:
// This would be an interesting use of lambdas to return the
// value for a given column, allowing a "grid_row" function to
// be produced:
// grid_row("Pix %d", int lambda(int j) { return div_pix[i][j]; })
for(j = 0; j < data->divisions; j++) {
grid_entry(data, "Pix %d", div_pix[i][j]);
}
puts("|");
for(j = 0; j < data->divisions; j++) {
grid_entry(data, "Avg %f", lutf(data, avg[i][j]));
}
puts("|");
for(j = 0; j < data->divisions; j++) {
grid_entry(data, "Min %f", data->depth_lut[min[i][j]]);
}
puts("|");
for(j = 0; j < data->divisions; j++) {
grid_entry(data, "Med ~%f", data->depth_lut[median[i][j]]);
}
puts("|");
for(j = 0; j < data->divisions; j++) {
grid_entry(data, "Max %f", data->depth_lut[max[i][j]]);
}
puts("|");
for(j = 0; j < data->divisions; j++) {
grid_entry(data, "Out %d%%", oor_count[i][j] * 100 / div_pix[i][j]);
}
puts("|");
break;
case HISTOGRAM:
for(histcount = 0; histcount < data->histrows; histcount++) {
for(j = 0; j < data->divisions; j++) {
int l, val = 0;
if(i != i && i == 2 && j == 4 && histcount == 0) { // XXX : this block is for debugging and won't be entered
printf("\n");
for(l = 0; l < SM_HIST_SIZE; l++) {
INFO_OUT("%d (%f): %d\n",
l * 1024 / SM_HIST_SIZE,
data->depth_lut[l * 1024 / SM_HIST_SIZE],
small_histogram[i][j][l]);
}
printf("\n");
}
for(l = 0; l < SM_HIST_SIZE / data->histrows; l++) {
val += small_histogram[i][j][histcount + l];
}
grid_bar(data, '*', val * 40 * data->histrows / div_pix[i][j]);
}
puts("|");
}
break;
case ASCII:
for(i = 0; i < data->divisions; i++) {
for(j = 0; j < data->divisions; j++) {
int c = (int)((data->depth_lut[min[i][j]] - data->zmin) *
4.0f / (data->zmax - data->zmin));
if(c > 5) {
c = 5;
} else if(c < 0) {
c = 0;
}
if(min[i][j] == 2047) {
c = 6;
}
// 1st character is closest, 5th character farthest
// 6th character is shown for out-of-range areas
putchar("8%+-._ "[c]);
}
putchar('\n');
}
break;
}
}
if(data->disp_mode != ASCII) {
grid_hline(data);
}
fflush(stdout);
// Make LED red if more than 35% of the image is out of range (can't
// set LED in callback for some reason)
data->out_of_range = oor_total > FREENECT_FRAME_PIX * 35 / 100;
data->frame++;
}
ImageReader::ImageHandle ImageReader::getImage(text::string_hash id)
{
if (hasProduct(id))
return getProduct(id);
// Image is not in cache, load it.
const std::string& url = text::get(id);
INFO_OUT(TAG, "Loading image %s", url.c_str());
FILE* fp = fopen(url.c_str(), "rb");
if (fp == NULL)
throw ResourceException("Cannot open file.");
if ( !isPNGFile(fp) )
{
fclose(fp);
throw ResourceException("File is not PNG.");
}
png_structp pngStruct = png_create_read_struct(PNG_LIBPNG_VER_STRING, nullptr, nullptr, nullptr);
if (!pngStruct)
{
fclose(fp);
throw ResourceException("[LIBPNG] Error creating read struct.");
}
png_infop pngInfo = png_create_info_struct(pngStruct);
if (!pngInfo)
{
png_destroy_read_struct(&pngStruct, nullptr, nullptr);
fclose(fp);
throw ResourceException("[LIBPNG] Error creating info struct.");
}
if ( setjmp(png_jmpbuf(pngStruct)) )
{
png_destroy_read_struct(&pngStruct, &pngInfo, nullptr);
fclose(fp);
throw ResourceException("Error while reading file.");
}
png_init_io(pngStruct, fp);
png_set_sig_bytes(pngStruct, pngHeaderCheckSize);
// FIXME Write directly to buffer, don't copy.
png_read_png(pngStruct,
pngInfo,
PNG_TRANSFORM_STRIP_16 | PNG_TRANSFORM_PACKING | PNG_TRANSFORM_EXPAND,
NULL);
struct
{
png_uint_32 width;
png_uint_32 height;
png_byte bitsPerPixel;
png_byte colorType;
png_size_t rowSize;
} imageData;
imageData.width = png_get_image_width(pngStruct, pngInfo);
imageData.height = png_get_image_height(pngStruct, pngInfo);
imageData.bitsPerPixel = png_get_bit_depth(pngStruct, pngInfo);
imageData.colorType = png_get_color_type(pngStruct, pngInfo);
imageData.rowSize = png_get_rowbytes(pngStruct, pngInfo);
// FIXME Let PixelFormat decide what format is suitable.
gr::PixelFormat fmt;
if (imageData.colorType == PNG_COLOR_TYPE_RGBA)
fmt = gr::PixelFormat::R8G8B8A8;
else
fmt = gr::PixelFormat::R8G8B8;
unsigned char* pixels = new unsigned char[imageData.rowSize * imageData.height];
png_bytepp rowPointers = png_get_rows(pngStruct, pngInfo);
for (unsigned int i = 0; i < imageData.height; i++)
memcpy(pixels + (imageData.rowSize * (imageData.height - 1 - i)),
rowPointers[i],
imageData.rowSize);
png_destroy_read_struct(&pngStruct, &pngInfo, NULL);
fclose(fp);
return addProduct(id, gr::Image((unsigned int) imageData.width,
(unsigned int) imageData.height,
fmt,
1u,
pixels));
}
